# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1313643818 14400
# Node ID 32d2722d40282be159bf5ab2490f7eb9c3fa6357
# Parent  a617d0808bbc6f71339dcaaf2018f8594a5203e7
added constraint on minimum displacement

diff -r a617d0808bbc -r 32d2722d4028 c/Feature.cpp
--- a/c/Feature.cpp	Wed Aug 17 23:21:26 2011 -0400
+++ b/c/Feature.cpp	Thu Aug 18 01:03:38 2011 -0400
@@ -12,6 +12,18 @@
   addPoint(frameNum, p);
 }
 
+bool FeatureTrajectory::largeDisplacement(const int& nDisplacements, const float& minTotalFeatureDisplacement) const {
+  bool result = true;
+  unsigned int nPositions = positions.size();
+  if (nPositions > nDisplacements) {
+    float disp = 0;
+    for (int i=0; i<nDisplacements; i++)
+      disp += displacementDistances[nPositions-2-i];
+    result = disp > minTotalFeatureDisplacement;
+  }
+  return result;
+}
+
 void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
   positions.add(frameNum, p);
   computeMotionData(frameNum);
@@ -40,15 +52,10 @@
   unsigned int nPositions = positions.size();
   if (nPositions >= 3) {
     Point2f displacement = positions[nPositions-1] - positions[nPositions-2];
+    if (nPositions == 2) // duplicate first displacement so that positions and velocities have the same length
+      velocities.add(frameNum-1, displacement);
     velocities.add(frameNum, displacement);
     float dist = norm(displacement);
     displacementDistances.push_back(dist);
-  } else if (nPositions == 2) {
-    Point2f displacement = positions[1] - positions[0];
-    velocities.add(frameNum-1, displacement);
-    velocities.add(frameNum, displacement);
-    float dist = norm(displacement);
-    displacementDistances.push_back(dist);
-    displacementDistances.push_back(dist);
   }
 }
diff -r a617d0808bbc -r 32d2722d4028 c/feature-based-tracking.cpp
--- 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());
diff -r a617d0808bbc -r 32d2722d4028 include/Feature.hpp
--- a/include/Feature.hpp	Wed Aug 17 23:21:26 2011 -0400
+++ b/include/Feature.hpp	Thu Aug 18 01:03:38 2011 -0400
@@ -18,6 +18,11 @@
 
   void setId(const unsigned int& id) { positions.setId(id);velocities.setId(id);}
 
+  /// indicates whether the sum of the last nDisplacements displacements have been superior to minFeatureDisplacement
+  bool largeDisplacement(const int& nDisplacements, const float& minTotalFeatureDisplacement) const;
+
+  //void shorten(void);
+
   void addPoint(const int& frameNum, const cv::Point2f& p);
 
   void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB) const;
@@ -38,4 +43,7 @@
 
 typedef boost::shared_ptr<FeatureTrajectory> FeatureTrajectoryPtr;
 
+// class MovingObject {}
+// roadUserType, group of features
+
 #endif
diff -r a617d0808bbc -r 32d2722d4028 include/Parameters.hpp
--- a/include/Parameters.hpp	Wed Aug 17 23:21:26 2011 -0400
+++ b/include/Parameters.hpp	Thu Aug 18 01:03:38 2011 -0400
@@ -23,7 +23,7 @@
   bool useHarrisDetector;
   float k;
   int pyramidLevel;
-  int nFramesDisplacement;
+  int nDisplacements;
   float minFeatureDisplacement;
   float accelerationBound;
   float deviationBound;
@@ -32,7 +32,7 @@
   int maxNumberTrackingIterations;
   float minTrackingError;
   float derivLambda;
-  int minFeatureTime;
+  unsigned int minFeatureTime;
   float mmConnectionDistance;
   float mmSegmentationDistance;
   float maxDistance;
@@ -41,7 +41,7 @@
 
   //KLTFeatureTrackingParameters(const int argc, char* argv[]);
 
-  //KLTFeatureTrackingParameters(bool loadFeatures, std::string videoFilename, int videoFPS, int measurementPrecision, int frame1, int nFrames, int maxNFeatures, float featureQuality, float minFeatureDistanceKLT, int windowSize, int pyramidLevel, int nFramesDisplacement, float minFeatureDisplacement, float accelerationBound, float deviationBound, int nFramesSmoothing, int nFramesVelocity, int maxNumberTrackingIterations, float minTrackingError, int minFeatureTime, float mmConnectionDistance, float mmSegmentationDistance, float maxDistance, float minVelocityCosine, int minNFeaturesPerGroup);
+  //KLTFeatureTrackingParameters(bool loadFeatures, std::string videoFilename, int videoFPS, int measurementPrecision, int frame1, int nFrames, int maxNFeatures, float featureQuality, float minFeatureDistanceKLT, int windowSize, int pyramidLevel, int nDisplacements, float minFeatureDisplacement, float accelerationBound, float deviationBound, int nFramesSmoothing, int nFramesVelocity, int maxNumberTrackingIterations, float minTrackingError, int minFeatureTime, float mmConnectionDistance, float mmSegmentationDistance, float maxDistance, float minVelocityCosine, int minNFeaturesPerGroup);
 
 };