--- a/c/feature-based-tracking.cpp	Wed Aug 17 23:21:26 2011 -0400
+++ b/c/feature-based-tracking.cpp	Thu Aug 18 01:03:38 2011 -0400
@@ -60,7 +60,7 @@
   Mat frame, currentFrameBW, previousFrameBW;
 
   KLTFeatureTrackingParameters params;
-  params.display = false;
+  params.display = true;
   params.frame1 = 0;
   params.nFrames = -1;
   params.maxNFeatures = 1000;
@@ -72,6 +72,10 @@
   //GoodFeaturesToTrackDetector detector(params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, params.windowSize, params.useHarrisDetector, params.k);
 
   params.pyramidLevel = 3;
+  params.nDisplacements = 3;
+  params.minFeatureDisplacement = 0.05;
+  float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
+
   params.maxNumberTrackingIterations = 20; // 30
   params.minTrackingError = 0.3; // 0.01
   params.derivLambda = 0.5;
@@ -148,19 +152,26 @@
 	vector<Point2f> trackedPts;
 	vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
 	while (iter != featurePointMatches.end()) {
+	  bool deleteFeature = false;
+	  
 	  if (status[iter->pointNum]) {
 	    iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
 	    trackedPts.push_back(currPts[iter->pointNum]);
 	    iter->pointNum = trackedPts.size()-1;
-	    iter++;
-	  } else {
+
+	    deleteFeature |= !iter->feature->largeDisplacement(params.nDisplacements, minTotalFeatureDisplacement);
+	    // motionSmooth()
+	  } 
+
+	  if (deleteFeature) {
 	    if (iter->feature->length() >= params.minFeatureTime) {
 	      iter->feature->setId(savedFeatureId);
 	      savedFeatureId++;
 	      iter->feature->write(trajectoryDB);
 	    }
 	    iter = featurePointMatches.erase(iter);
-	  }
+	  } else
+	    iter++;
 	}
 	currPts = trackedPts;
 	assert(currPts.size() == featurePointMatches.size());