--- a/c/Motion.cpp	Wed Jun 01 01:55:45 2016 -0400
+++ b/c/Motion.cpp	Wed Jun 01 17:57:49 2016 -0400
@@ -22,11 +22,11 @@
 
 /******************** FeatureTrajectory ********************/
 
-FeatureTrajectory::FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p, const Mat& homography)
+FeatureTrajectory::FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p)
   : firstInstant(frameNum), lastInstant(frameNum) {
   positions = TrajectoryPoint2fPtr(new TrajectoryPoint2f());
   velocities = TrajectoryPoint2fPtr(new TrajectoryPoint2f());
-  addPoint(frameNum, p, homography);
+  addPoint(frameNum, p);
 }
 
 FeatureTrajectory::FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities) {
@@ -93,11 +93,8 @@
   return connected;
 }
 
-void FeatureTrajectory::addPoint(const unsigned int& frameNum, const Point2f& p, const Mat& homography) {
-  Point2f pp = p;
-  if (!homography.empty())
-    pp = project(p, homography);
-  positions->add(frameNum, pp);
+void FeatureTrajectory::addPoint(const unsigned int& frameNum, const Point2f& p) {
+  positions->add(frameNum, p);
   if (frameNum < firstInstant)
     firstInstant = frameNum;
   if (frameNum > lastInstant)
@@ -128,12 +125,12 @@
 void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const {
   Point2f p1, p2;
   if (!homography.empty())
-    p1 = project((*positions)[0], homography);
+    p1 = project<double>((*positions)[0], homography);
   else
     p1 = (*positions)[0];
   for (unsigned int i=1; i<positions->size(); i++) {
     if (!homography.empty())
-      p2 = project((*positions)[i], homography);
+      p2 = project<double>((*positions)[i], homography);
     else
       p2 = (*positions)[i];
     line(img, p1, p2, color, 1);

--- a/c/cvutils.cpp	Wed Jun 01 01:55:45 2016 -0400
+++ b/c/cvutils.cpp	Wed Jun 01 17:57:49 2016 -0400
@@ -11,19 +11,18 @@
 using namespace std;
 using namespace cv;
 
-Point2f project(const Point2f& p, const Mat& homography) {
-  //Mat homogeneous(3, 1, CV_32FC1);
-  float x, y;
-  float w = homography.at<float>(2,0)*p.x+homography.at<float>(2,1)*p.y+homography.at<float>(2,2);
-  if (w != 0) {
-    x = (homography.at<float>(0,0)*p.x+homography.at<float>(0,1)*p.y+homography.at<float>(0,2))/w;
-    y = (homography.at<float>(1,0)*p.x+homography.at<float>(1,1)*p.y+homography.at<float>(1,2))/w;
-  } else {
-    x = 0;
-    y = 0;
-  }
-  return Point2f(x, y);
-}
+// Point2f project(const Point2f& p, const Mat& homography) {
+//   float x, y;
+//   float w = homography.at<float>(2,0)*p.x+homography.at<float>(2,1)*p.y+homography.at<float>(2,2);
+//   if (w != 0) {
+//     x = (homography.at<float>(0,0)*p.x+homography.at<float>(0,1)*p.y+homography.at<float>(0,2))/w;
+//     y = (homography.at<float>(1,0)*p.x+homography.at<float>(1,1)*p.y+homography.at<float>(1,2))/w;
+//   } else {
+//     x = 0;
+//     y = 0;
+//   }
+//   return Point2f(x, y);
+// }
 
 Mat loadMat(const string& filename, const string& separator) {
   vector<vector<float> > numbers = ::loadNumbers(filename, separator);

--- a/c/feature-based-tracking.cpp	Wed Jun 01 01:55:45 2016 -0400
+++ b/c/feature-based-tracking.cpp	Wed Jun 01 17:57:49 2016 -0400
@@ -64,13 +64,13 @@
 
 void trackFeatures(const KLTFeatureTrackingParameters& params) {
   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();
-
+  //if (params.stabilize && refHomography.empty())
+  //  refHomography = Mat::eye(3, 3, CV_64FC1);
+  //Mat stabilizationHomography(3,3,CV_64FC1), homography(3,3,CV_64FC1);
+  Mat stabilizationHomography, homography;
+  if (!params.stabilize)
+    homography = refHomography;
+  
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
 
@@ -131,6 +131,7 @@
     cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
     mask = Mat::ones(videoSize, CV_8UC1);
   }
+  Mat featureMask = mask.clone();
 
   std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
   //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
@@ -140,9 +141,9 @@
   trajectoryDB->beginTransaction();
 
   std::vector<KeyPoint> prevKpts, currKpts;
-  std::vector<Point2f> prevPts, currPts, newPts, homographyRefPts, homographyCurrPts;
-  std::vector<uchar> status, homographPtsStatus;
-  std::vector<float> errors, homographyErrors;
+  std::vector<Point2f> prevPts, currPts, newPts, stabilizationRefPts, stabilizationCurrPts;
+  std::vector<uchar> status, stabilizationPtsStatus;
+  std::vector<float> errorPts, stabilizationErrorPts;
   Mat prevDesc, currDesc;
 
   std::vector<FeatureTrajectoryPtr> lostFeatures;
@@ -163,7 +164,7 @@
       }
 
       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?
+      goodFeaturesToTrack(homographyFrameBW, stabilizationRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features?
     }
   }
 
@@ -171,6 +172,7 @@
   if (params.nFrames > 0)
     lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
 
+  // Main Loop
   capture.set(CV_CAP_PROP_POS_FRAMES, params.frame1);
   for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
     capture >> frame;
@@ -193,36 +195,57 @@
     
     if (!prevPts.empty()) {
       currPts.clear();
-      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);
+      calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errorPts, 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.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);
+	  calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, stabilizationRefPts, stabilizationCurrPts, stabilizationPtsStatus, stabilizationErrorPts, 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]);
+	  std::vector<Point2f> stabilizationRefPts2, stabilizationCurrPts2;
+	  for (unsigned int i=0; i<stabilizationRefPts.size(); i++)
+	    if (stabilizationPtsStatus[i]) {
+	      stabilizationRefPts2.push_back(stabilizationRefPts[i]);
+	      stabilizationCurrPts2.push_back(stabilizationCurrPts[i]);
 	    }
-	  stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold);
-	  if (stabilizationHomography.empty())
+	  stabilizationHomography = findHomography(stabilizationCurrPts2, stabilizationRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold);
+	  //cout << stabilizationHomography << endl;
+	  //cout << stabilizationHomography.type() << " " << refHomography.type() << endl;
+	  //for (unsigned int i=0; i<MIN(stabilizationRefPts.size(), 10); i++) {
+	  //cout << stabilizationCurrPts2[i] << " " << stabilizationRefPts2[i] << ": projected " << ::project<double>(stabilizationCurrPts2[i], stabilizationHomography) << endl;
+	    //cout << stabilizationHomography.at<double>(0,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(0,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(0,2) << " " << stabilizationHomography.at<double>(1,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(1,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(1,2) << " " << stabilizationHomography.at<double>(2,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(2,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(2,2) << endl;
+	  //}
+	  if (stabilizationHomography.empty()) {
 	    homography = refHomography;
-	  else
-	    homography = stabilizationHomography*refHomography;
-      }
+	    featureMask = mask.clone();
+	  } else {
+	    if (refHomography.empty())
+	      homography = stabilizationHomography;
+	    else
+	      homography = refHomography*stabilizationHomography;
+	    //cout << refHomography << " * " << stabilizationHomography << " = " << homography << endl;
+	    warpPerspective(mask, featureMask, stabilizationHomography, videoSize, INTER_LINEAR+WARP_INVERSE_MAP, BORDER_CONSTANT, 0);
+	  }
+      } else
+	featureMask = mask.clone();
+
       
       std::vector<Point2f> trackedPts;
       std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
       while (iter != featurePointMatches.end()) {
 	bool deleteFeature = false;
-	  
-	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)
-	    iter->feature->shorten();
+	
+	if (status[iter->pointNum]) {
+	  Point2f homographyFramePt = ::project<double>(currPts[iter->pointNum], stabilizationHomography); // project to space of homography frame (if not stabilization, stabilizationHomography should be empty)
+	  cout << currPts[iter->pointNum] << ", " << homographyFramePt << endl;
+	  if (::inImage(homographyFramePt, videoSize) && (mask.at<uchar>(static_cast<int>(round(homographyFramePt.y)), static_cast<int>(round(homographyFramePt.x))) != 0)) { // check point is in mask
+	    iter->feature->addPoint(frameNum, ::project<double>(homographyFramePt, refHomography));
+	    //cout << *(iter->feature) << endl;
+	    deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
+	      || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
+	    if (deleteFeature)
+	      iter->feature->shorten();
+	  } else
+	    deleteFeature = true;
 	} else
 	  deleteFeature = true;
 
@@ -245,27 +268,34 @@
       saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
 	
       if (params.display) {
-	cout << featurePointMatches.size() << endl;
+	cout << featurePointMatches.size() << " matches" << endl;
+	Mat invHomography;
+	if (!homography.empty())
+	  invHomography = homography.inv();
 	BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
-	  fp.feature->draw(frame, homography.inv(), Colors::red());
+	  fp.feature->draw(frame, invHomography, Colors::red());
       }
     }
     
     // adding new features, using mask around existing feature positions
-    Mat featureMask = mask.clone();
     for (unsigned int n=0;n<currPts.size(); n++)
       for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
 	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);
     BOOST_FOREACH(Point2f p, newPts) {
-      FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
+      FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, ::project<double>(::project<double>(p, stabilizationHomography), refHomography)));
       featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
       currPts.push_back(p);
     }
       
     if (params.display) {
-      imshow("mask", featureMask*256);
+      cout << currPts.size() << " current points" << endl;
+      Mat maskedFrame;
+      cvtColor(featureMask*256, maskedFrame, CV_GRAY2RGB);
+      addWeighted(frame, 0.5, maskedFrame, 0.5, 0.0, maskedFrame);
+      imshow("masked frame", maskedFrame);
+      //imshow("mask", featureMask*256);
       imshow("frame", frame);
       if (params.stabilize && !stabilizationHomography.empty()) {
 	Mat warped, vis;
@@ -273,7 +303,7 @@
 	addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis);
 	imshow("warped frame", vis);
       }
-      key = waitKey(2);
+      key = waitKey(0);
     }
     previousFrameBW = currentFrameBW.clone();
     prevPts = currPts;

--- a/include/Motion.hpp	Wed Jun 01 01:55:45 2016 -0400
+++ b/include/Motion.hpp	Wed Jun 01 17:57:49 2016 -0400
@@ -15,7 +15,7 @@
 class FeatureTrajectory 
 {
 public:
-  FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p, const cv::Mat& homography);
+  FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p);
 
   FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities);
 
@@ -43,7 +43,7 @@
   /// computes the distance according to the Beymer et al. algorithm
   bool minMaxSimilarity(const FeatureTrajectory& ft, const int& firstInstant, const int& lastInstant, const float& connectionDistance, const float& segmentationDistance);
 
-  void addPoint(const unsigned int& frameNum, const cv::Point2f& p, const cv::Mat& homography);
+  void addPoint(const unsigned int& frameNum, const cv::Point2f& p);
 
   void shorten(void);
 

--- a/include/cvutils.hpp	Wed Jun 01 01:55:45 2016 -0400
+++ b/include/cvutils.hpp	Wed Jun 01 17:57:49 2016 -0400
@@ -5,28 +5,38 @@
 #include "opencv2/features2d/features2d.hpp"
 
 class CvCapture;
-//template<typename T> class Point_<T>;
-
-/// constant that indicates if the image should be flipped
-//static const int flipImage = CV_CVTIMG_FLIP;
 
 /** Projects a point with the homography matrix
  use perspectiveTransform for arrays of points. */
-cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography);
+//cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography);
+template<typename T>
+inline cv::Point_<T> project(const cv::Point_<T>& p, const cv::Mat_<T>& homography) {
+  if (homography.empty())
+    return p;
+  else {
+    T x, y;
+    T w = homography.template at<T>(2,0)*p.x+homography.template at<T>(2,1)*p.y+homography.template at<T>(2,2);
+    if (w != 0.) {
+      x = (homography.template at<T>(0,0)*p.x+homography.template at<T>(0,1)*p.y+homography.template at<T>(0,2))/w;
+      y = (homography.template at<T>(1,0)*p.x+homography.template at<T>(1,1)*p.y+homography.template at<T>(1,2))/w;
+    } else {
+      x = 0.;
+      y = 0.;
+    }
+    return cv::Point_<T>(x, y);
+  }
+}
 
 /** Loads a cv mat from a text file where the numbers are saved line by line separated by separator */
 cv::Mat loadMat(const std::string& filename, const std::string& separator);
 
+inline bool inImage(const  cv::Point2f& p, const cv::Size s) { return (p.x >= 0) && (p.x <= s.width) && (p.y >= 0) && (p.y <= s.height); }
+
 //template<typename T> 
 //float scalarProduct(const cv::Point_<T>& v1, const cv::Point_<T>& v2) { return v1.x*v2.x+v1.y*v2.y;}
 
 void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts, const bool& clearPts = true);
 
-/** Allocates a new IplImage. */
-// IplImage* allocateImage(const int& width, const int& height, const int& depth, const int& channels);
-
-// IplImage* allocateImage(const CvSize& size, const int& depth, const int& channels);
-
 /** Goes to the target frame number, by querying frame, 
     supposing the video input is currently at current frame number.
     Returns the frame number that was reached.*/