--- a/c/feature-based-tracking.cpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/c/feature-based-tracking.cpp	Tue Aug 30 13:38:31 2011 -0400
@@ -53,6 +53,7 @@
 };
 
 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName, const unsigned int& minNFeatures = 0) {
+  /// \todo smoothing
   if (features.size() >= minNFeatures) {
     BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName);
     features.clear();
@@ -70,8 +71,10 @@
   KLTFeatureTrackingParameters params(argc, argv);
   cout << params.parameterDescription << endl;
 
-  Mat m = ::loadMat(params.homographyFilename, " ");
-  //cout << m << endl;
+  Mat homography = ::loadMat(params.homographyFilename, " ");
+  Mat invHomography;
+  if (params.display && !homography.empty())
+    invHomography = homography.inv();
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
@@ -119,12 +122,10 @@
   //     return 1;
   //   }
 
-  // mask
   Mat mask = imread(params.maskFilename, 0);
   if (mask.empty())
     mask = Mat::ones(videoSize, CV_8UC1);
 
-  // database
   boost::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = boost::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
   //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
   trajectoryDB->connect(params.databaseFilename.c_str());
@@ -171,7 +172,7 @@
 	  bool deleteFeature = false;
 	  
 	  if (status[iter->pointNum]) {
-	    iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
+	    iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
 
 	    deleteFeature |= iter->feature->smallDisplacement(params.nDisplacements, minTotalFeatureDisplacement)
 	      || !iter->feature->motionSmooth(params.accelerationBound, params.deviationBound);
@@ -184,8 +185,6 @@
 	    if (iter->feature->length() >= params.minFeatureTime) {
 	      iter->feature->setId(savedFeatureId);
 	      savedFeatureId++;
-	      /// \todo smoothing
-	      //iter->feature->write(*trajectoryDB);
 	      lostFeatures.push_back(iter->feature);
 	    }
 	    iter = featurePointMatches.erase(iter);
@@ -201,7 +200,7 @@
 	
 	if (params.display) {
 	  BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
-	    fp.feature->draw(frame, Colors::red());
+	    fp.feature->draw(frame, invHomography, Colors::red());
 	}
 	//drawOpticalFlow(prevPts, currPts, status, frame);
 	
@@ -219,7 +218,7 @@
 	    featureMask.at<uchar>(i,j)=0;
       goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.windowSize, params.useHarrisDetector, params.k);
       BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
-	FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p));
+	FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
 	featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
 	currPts.push_back(p);
       }