public static void draw3dAxis(
Mat frame, CameraParameters cp, Scalar color, double height, Mat Rvec, Mat Tvec) {
// Mat objectPoints = new Mat(4,3,CvType.CV_32FC1);
MatOfPoint3f objectPoints = new MatOfPoint3f();
Vector<Point3> points = new Vector<Point3>();
points.add(new Point3(0, 0, 0));
points.add(new Point3(height, 0, 0));
points.add(new Point3(0, height, 0));
points.add(new Point3(0, 0, height));
objectPoints.fromList(points);
MatOfPoint2f imagePoints = new MatOfPoint2f();
Calib3d.projectPoints(
objectPoints, Rvec, Tvec, cp.getCameraMatrix(), cp.getDistCoeff(), imagePoints);
List<Point> pts = new Vector<Point>();
Converters.Mat_to_vector_Point(imagePoints, pts);
Core.line(frame, pts.get(0), pts.get(1), color, 2);
Core.line(frame, pts.get(0), pts.get(2), color, 2);
Core.line(frame, pts.get(0), pts.get(3), color, 2);
Core.putText(frame, "X", pts.get(1), Core.FONT_HERSHEY_SIMPLEX, 0.5, color, 2);
Core.putText(frame, "Y", pts.get(2), Core.FONT_HERSHEY_SIMPLEX, 0.5, color, 2);
Core.putText(frame, "Z", pts.get(3), Core.FONT_HERSHEY_SIMPLEX, 0.5, color, 2);
}
/**
* Analyze video frames using computer vision approach and generate a ArrayList<AttitudeRec>
*
* @param recs output ArrayList of AttitudeRec
* @return total number of frame of the video
*/
private int analyzeVideo(ArrayList<AttitudeRec> recs) {
VideoMetaInfo meta = new VideoMetaInfo(new File(mPath, "videometa.json"));
int decimation = 1;
if (meta.fps > DECIMATION_FPS_TARGET) {
decimation = (int) (meta.fps / DECIMATION_FPS_TARGET);
meta.fps /= decimation;
}
VideoDecoderForOpenCV videoDecoder =
new VideoDecoderForOpenCV(
new File(mPath, "video.mp4"), decimation); // every 3 frame process 1 frame
Mat frame;
Mat gray = new Mat();
int i = -1;
Size frameSize = videoDecoder.getSize();
if (frameSize.width != meta.frameWidth || frameSize.height != meta.frameHeight) {
// this is very unlikely
return -1;
}
if (TRACE_VIDEO_ANALYSIS) {
Debug.startMethodTracing("cvprocess");
}
Size patternSize = new Size(4, 11);
float fc = (float) (meta.frameWidth / 2.0 / Math.tan(meta.fovWidth / 2.0));
Mat camMat = cameraMatrix(fc, new Size(frameSize.width / 2, frameSize.height / 2));
MatOfDouble coeff = new MatOfDouble(); // dummy
MatOfPoint2f centers = new MatOfPoint2f();
MatOfPoint3f grid = asymmetricalCircleGrid(patternSize);
Mat rvec = new MatOfFloat();
Mat tvec = new MatOfFloat();
MatOfPoint2f reprojCenters = new MatOfPoint2f();
if (LOCAL_LOGV) {
Log.v(TAG, "Camera Mat = \n" + camMat.dump());
}
long startTime = System.nanoTime();
while ((frame = videoDecoder.getFrame()) != null) {
if (LOCAL_LOGV) {
Log.v(TAG, "got a frame " + i);
}
// has to be in front, as there are cases where execution
// will skip the later part of this while
i++;
// convert to gray manually as by default findCirclesGridDefault uses COLOR_BGR2GRAY
Imgproc.cvtColor(frame, gray, Imgproc.COLOR_RGB2GRAY);
boolean foundPattern =
Calib3d.findCirclesGridDefault(
gray, patternSize, centers, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
if (!foundPattern) {
// skip to next frame
continue;
}
if (OUTPUT_DEBUG_IMAGE) {
Calib3d.drawChessboardCorners(frame, patternSize, centers, true);
}
// figure out the extrinsic parameters using real ground truth 3D points and the pixel
// position of blobs found in findCircleGrid, an estimated camera matrix and
// no-distortion are assumed.
boolean foundSolution =
Calib3d.solvePnP(grid, centers, camMat, coeff, rvec, tvec, false, Calib3d.CV_ITERATIVE);
if (!foundSolution) {
// skip to next frame
if (LOCAL_LOGV) {
Log.v(TAG, "cannot find pnp solution in frame " + i + ", skipped.");
}
continue;
}
// reproject points to for evaluation of result accuracy of solvePnP
Calib3d.projectPoints(grid, rvec, tvec, camMat, coeff, reprojCenters);
// error is evaluated in norm2, which is real error in pixel distance / sqrt(2)
double error = Core.norm(centers, reprojCenters, Core.NORM_L2);
if (LOCAL_LOGV) {
Log.v(TAG, "Found attitude, re-projection error = " + error);
}
// if error is reasonable, add it into the results
if (error < REPROJECTION_THREASHOLD) {
double[] rv = new double[3];
rvec.get(0, 0, rv);
recs.add(new AttitudeRec((double) i / meta.fps, rodr2rpy(rv)));
}
if (OUTPUT_DEBUG_IMAGE) {
Calib3d.drawChessboardCorners(frame, patternSize, reprojCenters, true);
Highgui.imwrite(
Environment.getExternalStorageDirectory().getPath()
+ "/RVCVRecData/DebugCV/img"
+ i
+ ".png",
frame);
}
}
if (LOCAL_LOGV) {
Log.v(TAG, "Finished decoding");
}
if (TRACE_VIDEO_ANALYSIS) {
Debug.stopMethodTracing();
}
if (LOCAL_LOGV) {
// time analysis
double totalTime = (System.nanoTime() - startTime) / 1e9;
Log.i(TAG, "Total time: " + totalTime + "s, Per frame time: " + totalTime / i);
}
return i;
}