# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1499724281 14400
# Node ID dbd81710d515cf2137cb55bf79a515030fd8b56c
# Parent  974077e23804636ab90801a51526efaffa944da4
new feature tracking in image space with point undistortion

diff -r 974077e23804 -r dbd81710d515 c/feature-based-tracking.cpp
--- a/c/feature-based-tracking.cpp	Mon Jul 10 01:38:12 2017 -0400
+++ b/c/feature-based-tracking.cpp	Mon Jul 10 18:04:41 2017 -0400
@@ -114,10 +114,6 @@
   Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix;
   if (params.undistort) {
     intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
-    //videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
-    // newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); 
-    // newIntrinsicCameraMatrix.at<float>(0,2) = undistortedVideoSize.width/2.;
-    // newIntrinsicCameraMatrix.at<float>(1,2) = undistortedVideoSize.height/2.;
     Size undistortedVideoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
     newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);//getOptimalNewCameraMatrix(intrinsicCameraMatrix, params.distortionCoefficients, videoSize, 1, undistortedVideoSize);//, 0, true);
     initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1) /* 0 ?*/, newIntrinsicCameraMatrix, undistortedVideoSize, CV_32FC1, map1, map2);
@@ -133,14 +129,12 @@
   }
 
   std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
-  //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
   trajectoryDB->connect(params.databaseFilename.c_str());
   trajectoryDB->createTable("positions");
   trajectoryDB->createTable("velocities");
   trajectoryDB->beginTransaction();
 
-  std::vector<KeyPoint> prevKpts, currKpts;
-  std::vector<Point2f> prevPts, currPts, newPts, undistortedPts;
+  std::vector<Point2f> prevPts, currPts, newPts, undistortedPts; // all points but undistortedPts are in image space
   std::vector<uchar> status;
   std::vector<float> errors;
   Mat prevDesc, currDesc;
@@ -164,16 +158,6 @@
       break;
     } else if (frameNum%50 ==0)
       cout << "frame " << frameNum << endl;
-
-    // if (params.undistort) {
-    //   remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
-    //   frame = undistortedFrame;
-
-    //   if (frame.size() != videoSize) {
-    // 	cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl;
-    // 	break;
-    //   }
-    // }
     
     cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
     
@@ -182,10 +166,10 @@
       calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
       /// \todo try calcOpticalFlowFarneback
 
-      if (params.undistort) {
+      if (params.undistort)
 	undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
-	//currPts = undistortedPts;
-      }
+      else
+	undistortedPts =currPts;
       
       std::vector<Point2f> trackedPts;
       std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
@@ -197,11 +181,8 @@
 	if ((status[iter->pointNum] =! 0) && 
 	    (currPtX >= 0) && (currPtX < videoSize.width) && 
 	    (currPtY >= 0) && (currPtY < videoSize.height) && 
-	    (mask.at<uchar>(currPtY, currPtX) != 0)) { // todo check point in mask in image space
-	  if (params.undistort)
-	    iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography);
-	  else
-	    iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
+	    (mask.at<uchar>(currPtY, currPtX) != 0)) {
+	  iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography);
 	  
 	  deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
 	    || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
@@ -246,21 +227,17 @@
 	for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
 	  featureMask.at<uchar>(i,j)=0;
     goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
-    if (params.undistort) {
+    if (params.undistort)
       undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
-      //newPts = undistortedPts;
-    }
-    //BOOST_FOREACH(Point2f p, newPts) {
+    else
+      undistortedPts = newPts;
+	
     for (unsigned int i=0; i<newPts.size(); i++) {
-      FeatureTrajectoryPtr f;
-      if (params.undistort) // write function
-	f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography));
-      else
-	f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, newPts[i], homography));
+      FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography));
       featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
       currPts.push_back(newPts[i]);
     }
-      
+  
     if (params.display && !displayFrame.empty()) {
       imshow("mask", featureMask*256);
       imshow("frame", displayFrame);
diff -r 974077e23804 -r dbd81710d515 python/cvutils.py
--- a/python/cvutils.py	Mon Jul 10 01:38:12 2017 -0400
+++ b/python/cvutils.py	Mon Jul 10 18:04:41 2017 -0400
@@ -122,9 +122,6 @@
 
     def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients):
         newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication)))
-        #newCameraMatrix = deepcopy(intrinsicCameraMatrix)
-        #newCameraMatrix[0,2] = newImgSize[0]/2.
-        #newCameraMatrix[1,2] = newImgSize[1]/2.
         newCameraMatrix = cv2.getDefaultNewCameraMatrix(intrinsicCameraMatrix, newImgSize, True)
         return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), None, newCameraMatrix, newImgSize, cv2.CV_32FC1)
 
@@ -432,7 +429,7 @@
                     invMap2[y,x] = res[1]
         return invMap1, invMap2
 
-    def cameraIntrinsicCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False):
+    def intrinsicCameraCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False, fixK2 = True, fixK3 = True, zeroTangent = True):
         ''' Camera calibration searches through all the images (jpg or png) located
         in _path_ for matches to a checkerboard pattern of size checkboardSize.
         These images should all be of the same camera with the same resolution.
@@ -491,10 +488,18 @@
         if len(objpoints) == 0 or len(imgpoints) == 0: 
             return None
         try:
-            ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
+            flags = 0
+            if fixK2:
+                flags += cv2.cv.CV_CALIB_FIX_K2
+            if fixK3:
+                flags += cv2.cv.CV_CALIB_FIX_K3
+            if zeroTangent:
+                flags += cv2.cv.CV_CALIB_ZERO_TANGENT_DIST
+            ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None, flags = flags)
         except NameError:
             return None
         savetxt('intrinsic-camera.txt', camera_matrix)
+        print 'error: {}'.format(ret)
         return camera_matrix, dist_coeffs
 
     def undistortImage(img, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., interpolation=cv2.INTER_LINEAR):
diff -r 974077e23804 -r dbd81710d515 scripts/undistort-video.py
--- a/scripts/undistort-video.py	Mon Jul 10 01:38:12 2017 -0400
+++ b/scripts/undistort-video.py	Mon Jul 10 18:04:41 2017 -0400
@@ -16,6 +16,7 @@
 parser.add_argument('--intrinsic', dest = 'intrinsicCameraMatrixFilename', help = 'name of the intrinsic camera file')
 parser.add_argument('--distortion-coefficients', dest = 'distortionCoefficients', help = 'distortion coefficients', nargs = '*', type = float)
 parser.add_argument('--undistorted-multiplication', dest = 'undistortedImageMultiplication', help = 'undistorted image multiplication', type = float)
+parser.add_argument('--mask', dest = 'maskFilename', help = 'name of the mask file, to undistort to see how it covers the undistortion errors')
 parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', type = int, default = 0)
 parser.add_argument('-l', dest = 'lastFrameNum', help = 'number of last frame number to save', type = int)
 parser.add_argument('-d', dest = 'destinationDirname', help = 'name of the directory where the undistorted frames are saved')
@@ -26,9 +27,6 @@
 args = parser.parse_args()
 
 intrinsicCameraMatrix = np.loadtxt(args.intrinsicCameraMatrixFilename)
-#distortionCoefficients = args.distortionCoefficients
-#undistortedImageMultiplication = args.undistortedImageMultiplication
-#firstFrameNum = params.firstFrameNum
 if args.destinationDirname is None:
     destinationDirname = ''
 else:
@@ -43,6 +41,9 @@
 width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH))
 height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT))
 [map1, map2] = cvutils.computeUndistortMaps(width, height, args.undistortedImageMultiplication, intrinsicCameraMatrix, args.distortionCoefficients)
+if args.maskFilename is not None:
+    mask = cv2.imread(args.maskFilename)
+    undistortedMask = cv2.remap(mask, map1, map2, interpolation=cv2.INTER_LINEAR)/255
 
 if capture.isOpened():
     ret = True
@@ -59,6 +60,8 @@
         if ret:
             img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR)
             cv2.imwrite(destinationDirname+'undistorted-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), img)
+            if args.maskFilename is not None:
+                cv2.imwrite(destinationDirname+'undistorted+mask-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), cv2.multiply(img, undistortedMask))
         frameNum += 1
 
 if args.encodeVideo: