--- a/c/Motion.cpp	Fri Aug 19 12:15:23 2011 -0400
+++ b/c/Motion.cpp	Tue Aug 23 13:14:47 2011 -0400
@@ -1,4 +1,5 @@
 #include "Motion.hpp"
+#include "cvutils.hpp"
 
 #include "opencv2/core/core.hpp"
 #include "opencv2/highgui/highgui.hpp"
@@ -24,9 +25,28 @@
   return result;
 }
 
+bool FeatureTrajectory::motionSmooth(const int& accelerationBound, const int& deviationBound) const {
+  bool result = true;
+  unsigned int nPositions = positions.size();
+  if (nPositions >= 3) {
+    float ratio;
+    if (displacementDistances[nPositions-2] > displacementDistances[nPositions-3])
+      ratio = displacementDistances[nPositions-2] / displacementDistances[nPositions-3];
+    else
+      ratio = displacementDistances[nPositions-3] / displacementDistances[nPositions-2];
+
+    float cosine = scalarProduct(velocities[nPositions-3],velocities[nPositions-2]) / (displacementDistances[nPositions-3] * displacementDistances[nPositions-2]);
+    
+    result &= (ratio < accelerationBound) & (cosine > deviationBound);
+  }
+  return result;
+}
+
 void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
   positions.add(frameNum, p);
   computeMotionData(frameNum);
+  assert(positions.size() == displacementDistances.size()+1);
+  assert(positions.size() == velocities.size()+1);
 }
 
 void FeatureTrajectory::shorten(void) { 
@@ -56,10 +76,10 @@
 
 void FeatureTrajectory::computeMotionData(const int& frameNum) {
   unsigned int nPositions = positions.size();
-  if (nPositions >= 3) {
+  if (nPositions >= 2) {
     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);
+    //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);

--- a/c/feature-based-tracking.cpp	Fri Aug 19 12:15:23 2011 -0400
+++ b/c/feature-based-tracking.cpp	Tue Aug 23 13:14:47 2011 -0400
@@ -62,24 +62,6 @@
 
   KLTFeatureTrackingParameters params(argc, argv);
   cout << params.parameterDescription << endl;
-  // params.display = true;
-  // params.frame1 = 0;
-  // params.nFrames = -1;
-  // params.maxNFeatures = 1000;
-  // params.featureQuality = 0.1;
-  // params.minFeatureDistanceKLT = 3;
-  // params.windowSize = 3; 
-  // params.useHarrisDetector = false;
-  // params.k = 0.4;
-  // //GoodFeaturesToTrackDetector detector(params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, params.windowSize, params.useHarrisDetector, params.k);
-
-  // params.pyramidLevel = 3;
-  // params.nDisplacements = 3;
-  // params.minFeatureDisplacement = 0.05;
-
-  // params.maxNumberTrackingIterations = 20; // 30
-  // params.minTrackingError = 0.3; // 0.01
-  // params.minFeatureTime = 20;
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
@@ -142,7 +124,6 @@
   vector<FeatureTrajectoryPtr> features;
   vector<FeaturePointMatch> featurePointMatches;
     
-  // TODO structure de donnee paires pointeur trajectory, numero de keypoint
   int key = '?';
   unsigned int savedFeatureId=0;
   for (int frameNum = params.frame1; ((params.frame1+frameNum < params.nFrames) || (params.nFrames < 0)) && !::interruptionKey(key); frameNum++) {
@@ -169,17 +150,18 @@
 	  if (status[iter->pointNum]) {
 	    iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
 
-	    bool smallDisplacement = iter->feature->smallDisplacement(params.nDisplacements, minTotalFeatureDisplacement);
-	    if (smallDisplacement)
+	    deleteFeature |= iter->feature->smallDisplacement(params.nDisplacements, minTotalFeatureDisplacement)
+	      || !iter->feature->motionSmooth(params.accelerationBound, params.deviationBound);
+	    if (deleteFeature)
 	      iter->feature->shorten();
-	    deleteFeature |= smallDisplacement;
-	    // motionSmooth()
-	  } 
+	  } else
+	    deleteFeature = true;
 
 	  if (deleteFeature) {
 	    if (iter->feature->length() >= params.minFeatureTime) {
 	      iter->feature->setId(savedFeatureId);
 	      savedFeatureId++;
+	      /// \todo smoothing
 	      iter->feature->write(*trajectoryDB);
 	    }
 	    iter = featurePointMatches.erase(iter);
@@ -192,9 +174,10 @@
 	currPts = trackedPts;
 	assert(currPts.size() == featurePointMatches.size());
 	
-	if (params.display)
+	if (params.display) {
 	  BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
 	    fp.feature->draw(frame, Colors::red());
+	}
 	//drawOpticalFlow(prevPts, currPts, status, frame);
 	
 	// cout << matches.size() << " matches" << endl;

--- a/c/tracking.cfg	Fri Aug 19 12:15:23 2011 -0400
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,64 +0,0 @@
-# filename of the video to process
-video-filename = ~/Research/Data/minnesota/Rice-and-University-12_50.avi
-# filename of the database where results are saved
-database-filename = ~/Research/Data/minnesota/results.sqlite
-# filename of the homography matrix
-homography-filename = ~/Research/Data/minnesota/Rice-and-University-12_50-homography.txt
-# filename of the mask image (where features are detected)
-mask-filename = ~/Research/Data/minnesota/Rice-and-University-12_50-mask.png
-# load features from database
-load-features = false
-# display trajectories on the video
-display = false
-# original video frame rate
-video-fps = 29.97
-# number of digits of precision for all measurements derived from video
-# measurement-precision = 3
-# first frame to process
-frame1 = 0
-# number of frame to process
-nframes = -1
-# feature tracking
-# maximum number of features added at each frame
-max-nfeatures = 1000
-# quality level of the good features to track
-feature-quality = 0.1
-# minimum distance between features
-min-feature-distanceklt = 5
-# size of the search window at each pyramid level
-window-size = 7
-# use of Harris corner detector
-use-harris-detector = false
-# k parameter to detect good features to track (OpenCV)
-k = 0.4
-# maximal pyramid level in the feature tracking algorithm
-pyramid-level = 5
-# number of displacement to test minimum feature motion
-ndisplacements = 3
-# minimum displacement to keep features
-min-feature-displacement = 0.05
-# maximum feature acceleration
-acceleration-bound = 3
-# maximum feature deviation
-deviation-bound = 0.6
-# number of frames to smooth positions (half window)
-nframes-smoothing = 5
-# number of frames to compute velocities
-#nframes-velocity = 5
-# maximum number of iterations to stop feature tracking
-max-number-iterations = 20
-# minimum error to reach to stop feature tracking
-min-tracking-error = 0.3
-# minimum length of a feature (number of frames) to consider a feature for grouping
-min-feature-time = 20
-# Min Max similarity parameters (Beymer et al. method)
-# connection distance in feature grouping
-mm-connection-distance = 3.75
-# segmentation distance in feature grouping
-mm-segmentation-distance = 1.5
-# maximum distance between features for grouping
-max-distance = 5
-# minimum cosine of the angle between the velocity vectors for grouping
-min-velocity-cosine = 0.8
-# minimum average number of features per frame to create a vehicle hypothesis
-min-nfeatures-group = 3

--- a/include/Motion.hpp	Fri Aug 19 12:15:23 2011 -0400
+++ b/include/Motion.hpp	Tue Aug 23 13:14:47 2011 -0400
@@ -21,6 +21,9 @@
   /// indicates whether the sum of the last nDisplacements displacements has been inferior to minFeatureDisplacement
   bool smallDisplacement(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const;
 
+  /// indicates whether the last two displacements are smooth (limited acceleration and angle)
+  bool motionSmooth(const int& accelerationBound, const int& deviationBound) const;
+
   void addPoint(const int& frameNum, const cv::Point2f& p);
 
   void shorten(void);
@@ -33,8 +36,11 @@
 
 protected:
   Trajectory<cv::Point2f> positions;
+  /** one fewer velocity than position
+      v_n = p_n+1 - p_n*/
   Trajectory<cv::Point2f> velocities;
   
+  /// norms of velocities for feature constraints, one fewer positions than positions
   std::vector<float> displacementDistances;
 
   void computeMotionData(const int& frameNum);

--- a/include/cvutils.hpp	Fri Aug 19 12:15:23 2011 -0400
+++ b/include/cvutils.hpp	Tue Aug 23 13:14:47 2011 -0400
@@ -5,10 +5,14 @@
 #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;
 
+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. */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tracking.cfg	Tue Aug 23 13:14:47 2011 -0400
@@ -0,0 +1,64 @@
+# filename of the video to process
+video-filename = ~/Research/Data/minnesota/Rice-and-University-12_50.avi
+# filename of the database where results are saved
+database-filename = ~/Research/Data/minnesota/results.sqlite
+# filename of the homography matrix
+homography-filename = ~/Research/Data/minnesota/Rice-and-University-12_50-homography.txt
+# filename of the mask image (where features are detected)
+mask-filename = ~/Research/Data/minnesota/Rice-and-University-12_50-mask.png
+# load features from database
+load-features = false
+# display trajectories on the video
+display = false
+# original video frame rate
+video-fps = 29.97
+# number of digits of precision for all measurements derived from video
+# measurement-precision = 3
+# first frame to process
+frame1 = 0
+# number of frame to process
+nframes = -1
+# feature tracking
+# maximum number of features added at each frame
+max-nfeatures = 1000
+# quality level of the good features to track
+feature-quality = 0.1
+# minimum distance between features
+min-feature-distanceklt = 5
+# size of the search window at each pyramid level
+window-size = 7
+# use of Harris corner detector
+use-harris-detector = false
+# k parameter to detect good features to track (OpenCV)
+k = 0.4
+# maximal pyramid level in the feature tracking algorithm
+pyramid-level = 5
+# number of displacement to test minimum feature motion
+ndisplacements = 3
+# minimum displacement to keep features
+min-feature-displacement = 0.05
+# maximum feature acceleration
+acceleration-bound = 3
+# maximum feature deviation
+deviation-bound = 0.6
+# number of frames to smooth positions (half window)
+nframes-smoothing = 5
+# number of frames to compute velocities
+#nframes-velocity = 5
+# maximum number of iterations to stop feature tracking
+max-number-iterations = 20
+# minimum error to reach to stop feature tracking
+min-tracking-error = 0.3
+# minimum length of a feature (number of frames) to consider a feature for grouping
+min-feature-time = 20
+# Min Max similarity parameters (Beymer et al. method)
+# connection distance in feature grouping
+mm-connection-distance = 3.75
+# segmentation distance in feature grouping
+mm-segmentation-distance = 1.5
+# maximum distance between features for grouping
+max-distance = 5
+# minimum cosine of the angle between the velocity vectors for grouping
+min-velocity-cosine = 0.8
+# minimum average number of features per frame to create a vehicle hypothesis
+min-nfeatures-group = 3