--- a/c/Makefile	Tue May 31 17:59:35 2016 -0400
+++ b/c/Makefile	Wed Jun 01 01:55:45 2016 -0400
@@ -19,7 +19,7 @@
 
 ifneq ($(OPENCV), 0)
 	CFLAGS += -DUSE_OPENCV
-	LDFLAGS += -lopencv_highgui -lopencv_core -lopencv_video -lopencv_features2d -lopencv_imgproc -lopencv_imgcodecs -lopencv_videoio
+	LDFLAGS += -lopencv_highgui -lopencv_core -lopencv_video -lopencv_features2d -lopencv_imgproc -lopencv_imgcodecs -lopencv_videoio -lopencv_calib3d
 endif
 
 #LDFLAGS += -Wl,--as-needed -Wl,-Bdynamic,-lgcc_s,-Bstatic

--- a/c/Parameters.cpp	Tue May 31 17:59:35 2016 -0400
+++ b/c/Parameters.cpp	Wed Jun 01 01:55:45 2016 -0400
@@ -20,6 +20,7 @@
     ("gf", "groups features")
     ("loading-time", "report feature and object loading times")
     ("config-file", po::value<string>(&configurationFilename), "configuration file")
+    ("display", po::value<bool>(&display)->default_value(false), "display trajectories on the video")
     ;
 
   po::positional_options_description p;
@@ -36,8 +37,11 @@
     ("interpolation-method", po::value<int>(&interpolationMethod), "Interpolation method for remapping image when correcting for distortion: 0 for INTER_NEAREST - a nearest-neighbor interpolation; 1 for INTER_LINEAR - a bilinear interpolation (used by default); 2 for INTER_CUBIC - a bicubic interpolation over 4x4 pixel neighborhood; 3 for INTER_LANCZOS4")
     ("mask-filename", po::value<string>(&maskFilename), "filename of the mask image (where features are detected)")
     ("undistort", po::value<bool>(&undistort), "undistort the video for feature tracking")
-    ("load-features", po::value<bool>(&loadFeatures), "load features from database")
-    ("display", po::value<bool>(&display), "display trajectories on the video")
+    ("stabilize-ransac-reproj-threshold", po::value<float>(&stabilizeRansacReprojThreshold), "threshold the robust RANSAC method to compute the homography with respect to the reference video frame for stabilization")
+    ("frame-homography-filename", po::value<string>(&frameHomographyFilename), "filename of the video frame used to compute the homography")
+    ("stabilize", po::value<bool>(&stabilize), "stabilize the video for feature tracking (if slight motion)")
+    //("load-features", po::value<bool>(&loadFeatures), "load features from database")
+    //("display", po::value<bool>(&display), "display trajectories on the video")
     ("video-fps", po::value<float>(&videoFPS), "original video frame rate")
     ("frame1", po::value<unsigned int>(&frame1), "first frame to process")
     ("nframes", po::value<int>(&nFrames), "number of frame to process")

--- a/c/cvutils.cpp	Tue May 31 17:59:35 2016 -0400
+++ b/c/cvutils.cpp	Wed Jun 01 01:55:45 2016 -0400
@@ -31,7 +31,7 @@
   Mat mat;
 
   if (!numbers.empty()) {
-    mat = Mat(numbers.size(),numbers[0].size(), CV_32FC1);
+    mat = Mat(numbers.size(),numbers[0].size(), CV_64FC1);
     for (unsigned int i=0; i<numbers.size(); i++)
       for (unsigned int j=0; j<numbers[0].size(); j++)
 	mat.at<float>(i,j) = numbers[i][j];

--- a/c/feature-based-tracking.cpp	Tue May 31 17:59:35 2016 -0400
+++ b/c/feature-based-tracking.cpp	Wed Jun 01 01:55:45 2016 -0400
@@ -13,6 +13,7 @@
 #include "opencv2/features2d/features2d.hpp"
 #include "opencv2/highgui/highgui.hpp"
 #include "opencv2/objdetect/objdetect.hpp"
+#include <opencv2/calib3d.hpp>
 
 #include <boost/foreach.hpp>
 
@@ -62,10 +63,13 @@
 }
 
 void trackFeatures(const KLTFeatureTrackingParameters& params) {
-  Mat homography = ::loadMat(params.homographyFilename, " ");
-  Mat invHomography;
-  if (params.display && !homography.empty())
-    invHomography = homography.inv();
+  Mat refHomography = ::loadMat(params.homographyFilename, " ");
+  Mat stabilizationHomography, homography = refHomography;
+  if (params.stabilize && refHomography.empty())
+    refHomography = Mat::eye(3, 3, CV_32FC1);
+  //Mat invHomography;
+  //if (params.display && !homography.empty())
+  //  invHomography = homography.inv();
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
@@ -116,7 +120,7 @@
     videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
     newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.;
     newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.;
-    initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2);
+    initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3, 3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2);
     
     cout << "Undistorted width=" << videoSize.width <<
       ", height=" << videoSize.height << endl;
@@ -146,17 +150,23 @@
 
   int key = '?';
   unsigned int savedFeatureId=0;
-  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW;
+  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW, homographyFrame;
 
-  Mat homographyFrame = imread("/media/disk1/Research/Data/unb/Drone/S23-10-frame.png"); // reference frame for the homography, if option turned on
-  if (params.undistort) {
-    remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
-    homographyFrame = undistortedFrame;
+  if (params.stabilize) {
+    homographyFrame = imread(params.frameHomographyFilename);
+    if (homographyFrame.empty())
+      cout << "Homography frame filename " << params.frameHomographyFilename << " could not be opened." << endl;
+    else {
+      if (params.undistort) {
+	remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
+	homographyFrame = undistortedFrame;
+      }
+
+      cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY);  
+      goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features?
+    }
   }
 
-  cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY);  
-  goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features?
-
   unsigned int lastFrameNum = nFrames;
   if (params.nFrames > 0)
     lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
@@ -186,9 +196,22 @@
       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 there is stabilization todo
-      calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, homographyRefPts, homographyCurrPts, homographPtsStatus, homographyErrors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
+      if (params.stabilize) {
+	  calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, homographyRefPts, homographyCurrPts, homographPtsStatus, homographyErrors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
 
+	  std::vector<Point2f> homographyRefPts2, homographyCurrPts2;
+	  for (unsigned int i=0; i<homographyRefPts.size(); i++)
+	    if (homographPtsStatus[i]) {
+	      homographyRefPts2.push_back(homographyRefPts[i]);
+	      homographyCurrPts2.push_back(homographyCurrPts[i]);
+	    }
+	  stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold);
+	  if (stabilizationHomography.empty())
+	    homography = refHomography;
+	  else
+	    homography = stabilizationHomography*refHomography;
+      }
+      
       std::vector<Point2f> trackedPts;
       std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
       while (iter != featurePointMatches.end()) {
@@ -196,7 +219,6 @@
 	  
 	if (status[iter->pointNum] && (mask.at<uchar>(static_cast<int>(round(currPts[iter->pointNum].y)), static_cast<int>(round(currPts[iter->pointNum].x))) != 0)) {
 	  iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
-	  
 	  deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
 	    || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
 	  if (deleteFeature)
@@ -223,10 +245,9 @@
       saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
 	
       if (params.display) {
+	cout << featurePointMatches.size() << endl;
 	BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
-	  fp.feature->draw(frame, invHomography, Colors::red());
-	Mat homoFeatures = frame.clone();
-	// todo merge the ref frame and the current frame as in the Python example
+	  fp.feature->draw(frame, homography.inv(), Colors::red());
       }
     }
     
@@ -246,7 +267,12 @@
     if (params.display) {
       imshow("mask", featureMask*256);
       imshow("frame", frame);
-      //imshow("ref homography", )
+      if (params.stabilize && !stabilizationHomography.empty()) {
+	Mat warped, vis;
+	warpPerspective(frame, warped, stabilizationHomography, videoSize);
+	addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis);
+	imshow("warped frame", vis);
+      }
       key = waitKey(2);
     }
     previousFrameBW = currentFrameBW.clone();

--- a/include/Parameters.hpp	Tue May 31 17:59:35 2016 -0400
+++ b/include/Parameters.hpp	Wed Jun 01 01:55:45 2016 -0400
@@ -27,7 +27,10 @@
   int interpolationMethod;
   std::string maskFilename;
   bool undistort;
-  bool loadFeatures;
+  float stabilizeRansacReprojThreshold;
+  std::string frameHomographyFilename;
+  bool stabilize;
+  //bool loadFeatures;
   bool display;
   float videoFPS;
   // int measurementPrecision;

--- a/tracking.cfg	Tue May 31 17:59:35 2016 -0400
+++ b/tracking.cfg	Wed Jun 01 01:55:45 2016 -0400
@@ -20,10 +20,12 @@
 mask-filename = none
 # undistort the video for feature tracking
 undistort = false
-# load features from database
-load-features = false
-# display trajectories on the video
-display = false
+# threshold the robust RANSAC method to compute the homography with respect to the reference video frame for stabilization
+stabilize-ransac-reproj-threshold = 3.0
+# filename of the video frame used to compute the homography
+frame-homography-filename = none
+# stabilize the video for feature tracking (if slight motion)
+stabilize = false
 # original video frame rate (number of frames/s)
 video-fps = 29.97
 # number of digits of precision for all measurements derived from video