--- 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;