--- a/c/Parameters.cpp	Mon Apr 28 18:18:14 2014 -0400
+++ b/c/Parameters.cpp	Thu May 01 17:41:10 2014 -0400
@@ -33,33 +33,35 @@
     ("distortion-coefficients", po::value<std::vector<float> >(&distortionCoefficients)->multitoken(), "")
     ("undistorted-size-multiplication", po::value<float>(&undistortedImageMultiplication), "undistorted image multiplication")
     ("mask-filename", po::value<string>(&maskFilename), "filename of the mask image (where features are detected)")
+    ("undistort", po::value<bool>(&undistort), "undistort the video for feature tracking")
     ("load-features", po::value<bool>(&loadFeatures), "load features from database")
     ("display", po::value<bool>(&display), "display trajectories on the video")
     ("video-fps", po::value<float>(&videoFPS), "original video frame rate")
     ("frame1", po::value<unsigned int>(&frame1), "first frame to process")
     ("nframes", po::value<int>(&nFrames), "number of frame to process")
     // feature tracking
-    ("max-nfeatures", po::value<int>(&maxNFeatures), "maximum number of features added at each frame")
-    ("feature-quality", po::value<float>(&featureQuality), "quality level of the good features to track")
-    ("min-feature-distanceklt", po::value<float>(&minFeatureDistanceKLT), "minimum distance between features")
-    ("window-size", po::value<int>(&windowSize), "size of the search window at each pyramid level")
+    ("max-nfeatures", po::value<int>(&maxNFeatures), "maximum number of features added at each frame (1000s)")
+    ("feature-quality", po::value<float>(&featureQuality), "quality level of the good features to track (]0. 1?])")
+    ("min-feature-distanceklt", po::value<float>(&minFeatureDistanceKLT), "minimum distance between features (]0. 10?])")
+    ("block-size", po::value<int>(&blockSize), "size of the block for feature characteristics ([1 ?])")
     ("use-harris-detector", po::value<bool>(&useHarrisDetector), "use of Harris corner detector")
     ("k", po::value<float>(&k), "k parameter to detect good features to track (OpenCV)")
-    ("pyramid-level", po::value<int>(&pyramidLevel), "maximal pyramid level in the feature tracking algorithm")
-    ("ndisplacements", po::value<unsigned int>(&nDisplacements), "number of displacement to test minimum feature motion")
-    ("min-feature-displacement", po::value<float>(&minFeatureDisplacement), "minimum displacement to keep features")
-    ("acceleration-bound", po::value<float>(&accelerationBound), "maximum feature acceleration")
-    ("deviation-bound", po::value<float>(&deviationBound), "maximum feature deviation")
-    ("smoothing-halfwidth", po::value<int>(&nFramesSmoothing), "number of frames to smooth positions (half window)")
-    ("max-number-iterations", po::value<int>(&maxNumberTrackingIterations), "maximum number of iterations to stop feature tracking")
-    ("min-tracking-error", po::value<float>(&minTrackingError), "minimum error to reach to stop feature tracking")
-    ("min-feature-eig-threshold", po::value<float>(&minFeatureEigThreshold)->default_value(1e-4), "minimum eigen value of a 2x2 normal matrix of optical flow equations")
-    ("min-feature-time", po::value<unsigned int>(&minFeatureTime), "minimum length of a feature (number of frames) to consider a feature for grouping")
-    ("mm-connection-distance", po::value<float>(&mmConnectionDistance), "connection distance in feature grouping")
-    ("mm-segmentation-distance", po::value<float>(&mmSegmentationDistance), "segmentation distance in feature grouping")
-    ("max-distance", po::value<float>(&maxDistance), "maximum distance between features for grouping")
-    ("min-velocity-cosine", po::value<float>(&minVelocityCosine), "minimum cosine of the angle between the velocity vectors for grouping")
-    ("min-nfeatures-group", po::value<float>(&minNFeaturesPerGroup), "minimum average number of features per frame to create a vehicle hypothesis")
+    ("window-size", po::value<int>(&windowSize), "size of the search window at each pyramid level ([1 ?])")
+    ("pyramid-level", po::value<int>(&pyramidLevel), "maximal pyramid level in the feature tracking algorithm ([0 maxLevel=5?])")
+    ("ndisplacements", po::value<unsigned int>(&nDisplacements), "number of displacements to test minimum feature motion ([2 4])")
+    ("min-feature-displacement", po::value<float>(&minFeatureDisplacement), "minimum displacement per frame (in world space) to keep features (]0. 0.1?])")
+    ("acceleration-bound", po::value<float>(&accelerationBound), "maximum feature acceleration (]1 3+])")
+    ("deviation-bound", po::value<float>(&deviationBound), "maximum feature deviation (on cosine) (]0 1])")
+    ("smoothing-halfwidth", po::value<int>(&nFramesSmoothing), "number of frames to smooth positions (half window) ([0 inf[")
+    ("max-number-iterations", po::value<int>(&maxNumberTrackingIterations), "maximum number of iterations to stop optical flow (20-30?)")
+    ("min-tracking-error", po::value<float>(&minTrackingError), "minimum error to reach to stop optical flow (0.3-0.01)")
+    ("min-feature-eig-threshold", po::value<float>(&minFeatureEigThreshold)->default_value(1e-4), "minimum eigen value of a 2x2 normal matrix of optical flow equations (10^-4)")
+    ("min-feature-time", po::value<unsigned int>(&minFeatureTime), "minimum length of a feature (number of frames) to consider a feature for grouping [5 20+]")
+    ("mm-connection-distance", po::value<float>(&mmConnectionDistance), "connection distance in feature grouping (in world space) (ped: [0.5m 2m+], cars: [1.7m 4m+])")
+    ("mm-segmentation-distance", po::value<float>(&mmSegmentationDistance), "segmentation distance in feature grouping (in world space) (< mm-connection-distance, empirically ~ mm-connection-distance / 2.5)")
+    ("max-distance", po::value<float>(&maxDistance), "maximum distance between features for grouping (in world space) (unused)")
+    ("min-velocity-cosine", po::value<float>(&minVelocityCosine), "minimum cosine of the angle between the velocity vectors for grouping (unused)")
+    ("min-nfeatures-group", po::value<float>(&minNFeaturesPerGroup), "minimum average number of features per frame to create a vehicle hypothesis (]1 3+])")
     // ("max-uturn-cosine", po::value<float>(&maxUTurnCosine), "maximum cosine value to detect U-turn")
     // ("nframes-avoid-uturn", po::value<int>(&nFramesAvoidUTurn), "number of frames over which a feature should not make a U-turn")
     // Safety Analysis

--- a/c/feature-based-tracking.cpp	Mon Apr 28 18:18:14 2014 -0400
+++ b/c/feature-based-tracking.cpp	Thu May 01 17:41:10 2014 -0400
@@ -72,6 +72,8 @@
   Mat invHomography;
   if (params.display && !homography.empty())
     invHomography = homography.inv();
+  Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
+  //cout << intrinsicCameraMatrix << endl;
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
@@ -93,16 +95,31 @@
   }
   
   Size videoSize = capture->getSize();
+  //cout << capture->getSize() << " " << params.undistortedImageMultiplication*videoSize << endl;
   unsigned int nFrames = capture->getNbFrames();
   cout << "Video " << params.videoFilename <<
 	  ": width=" << videoSize.width <<
 	  ", height=" << videoSize.height <<
 	  ", nframes=" << nFrames << endl;
+
+  Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); 
+  Size newVideoSize = videoSize;
+  if (params.undistort) {
+    newVideoSize = Size(static_cast<int>(videoSize.width*params.undistortedImageMultiplication), static_cast<int>(videoSize.height*params.undistortedImageMultiplication));
+    newIntrinsicCameraMatrix.at<float>(0,2) = newVideoSize.width/2.;
+    newIntrinsicCameraMatrix.at<float>(1,2) = newVideoSize.height/2.;
+  }
   
+  Mat map1, map2;
+  Mat R = Mat::eye(3,3, CV_32FC1);
+  if (params.undistort)
+    initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, R, newIntrinsicCameraMatrix, newVideoSize, CV_32FC1, map1, map2);
+  
+  // todo mask in new size
   Mat mask = imread(params.maskFilename, 0);
   if (mask.empty()) {
     cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
-    mask = Mat::ones(videoSize, CV_8UC1);
+    mask = Mat::ones(newVideoSize, CV_8UC1);
   }
 
   boost::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = boost::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
@@ -121,23 +138,23 @@
   std::vector<FeatureTrajectoryPtr> lostFeatures;
   std::vector<FeaturePointMatch> featurePointMatches;
 
-  //HOGDescriptor hog;
-  //hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector());
-
   int key = '?';
   unsigned int savedFeatureId=0;
-  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW;
+  Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame;
 
   unsigned int lastFrameNum = nFrames;
   if (params.nFrames > 0)
     lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
-  
+
   capture->setFrameNumber(params.frame1);
   for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
       bool success = capture->getNextFrame(frame);
-
-      if (!success || frame.empty() || frame.size() != videoSize)
-	break;
+      if (params.undistort) {
+	remap(frame, undistortedFrame, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0.);
+	frame = undistortedFrame;
+      }
+      //if (!success || frame.empty() || frame.size() != videoSize)
+      //break;
 
       if (frameNum%50 ==0)
 	cout << "frame " << frameNum << endl;
@@ -146,7 +163,7 @@
       
       if (!prevPts.empty()) {
 	currPts.clear();
-	calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(3 /*static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS)*/, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
+	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);
 	/// \todo try calcOpticalFlowFarneback
 
 	std::vector<Point2f> trackedPts;
@@ -199,7 +216,7 @@
 	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.windowSize, params.useHarrisDetector, params.k);
+      goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, 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, homography));
 	featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));

--- a/include/Parameters.hpp	Mon Apr 28 18:18:14 2014 -0400
+++ b/include/Parameters.hpp	Thu May 01 17:41:10 2014 -0400
@@ -24,6 +24,7 @@
   std::vector<float> distortionCoefficients;
   float undistortedImageMultiplication;
   std::string maskFilename;
+  bool undistort;
   bool loadFeatures;
   bool display;
   float videoFPS;
@@ -31,27 +32,50 @@
   unsigned int frame1;
   int nFrames;
   // feature tracking
+  /// "Maximum number of corners to return" (OpenCV goodFeaturesToTrack) (should be large enough not to limit the potential number of features)
   int maxNFeatures;
+  /// "Parameter characterizing the minimal accepted quality of image corners" (OpenCV goodFeaturesToTrack )
   float featureQuality;
+  /// "Minimum possible Euclidean distance between the returned corners" (OpenCV goodFeaturesToTrack)
   float minFeatureDistanceKLT;
-  int windowSize;
+  /// "Size of an average block for computing a derivative covariation matrix over each pixel neighborhood" (OpenCV goodFeaturesToTrack)
+  int blockSize;
+  /// "Parameter indicating whether to use a Harris detector" (OpenCV goodFeaturesToTrack)
   bool useHarrisDetector;
+  /// "Free parameter of the Harris detector" (OpenCV goodFeaturesToTrack)
   float k;
+  /// "size of the search window at each pyramid level" (OpenCV calcOpticalFlowPyrLK)
+  int windowSize;
+  /// "0-based maximal pyramid level number" (OpenCV calcOpticalFlowPyrLK) higher is higher quality
   int pyramidLevel;
+  /// Number of displacements (number of frames-1) over which minimum motion is computed 
   unsigned int nDisplacements;
+  /// Minimum displacement per frame (in world space) to keep features
   float minFeatureDisplacement;
+  /// Maximum feature acceleration
   float accelerationBound;
+  /// Maximum feature deviation
   float deviationBound;
+  /// Number of frames to smooth positions (half window)
   int nFramesSmoothing;
   //int nFramesVelocity;
+  /// Maximum number of iterations to stop optical flow (OpenCV calcOpticalFlowPyrLK)
   int maxNumberTrackingIterations;
+  /// Minimum error to reach to stop optical flow (OpenCV calcOpticalFlowPyrLK)
   float minTrackingError;
+  /// Minimum eigen value of a 2x2 normal matrix of optical flow equations (OpenCV calcOpticalFlowPyrLK)
   float minFeatureEigThreshold;
+  /// Minimum length of a feature (number of frames) to consider a feature for grouping
   unsigned int minFeatureTime;
+  /// Connection distance in feature grouping (in world space)
   float mmConnectionDistance;
+  /// Segmentation distance in feature grouping (in world space)
   float mmSegmentationDistance;
+  /// Maximum distance between features for grouping (in world space) (unused)
   float maxDistance;
+  /// Minimum cosine of the angle between the velocity vectors for grouping (unused)
   float minVelocityCosine;
+  /// Minimum average number of features per frame to create a vehicle hypothesis
   float minNFeaturesPerGroup;
   // safety analysis
   float maxPredictedSpeed;

--- a/tracking.cfg	Mon Apr 28 18:18:14 2014 -0400
+++ b/tracking.cfg	Thu May 01 17:41:10 2014 -0400
@@ -35,12 +35,14 @@
 feature-quality = 0.1
 # minimum distance between features
 min-feature-distanceklt = 5
-# size of the search window at each pyramid level
-window-size = 7
+# size of the block for feature characteristics
+block-size = 7
 # use of Harris corner detector
 use-harris-detector = false
 # k parameter to detect good features to track (OpenCV)
 k = 0.4
+# size of the search window at each pyramid level
+window-size = 7
 # maximal pyramid level in the feature tracking algorithm
 pyramid-level = 5
 # number of displacement to test minimum feature motion
@@ -56,9 +58,9 @@
 # number of frames to compute velocities
 #nframes-velocity = 5
 # maximum number of iterations to stop feature tracking
-max-number-iterations = 20
+max-number-iterations = 30
 # minimum error to reach to stop feature tracking
-min-tracking-error = 0.3
+min-tracking-error = 0.01
 # minimum eigen value of a 2x2 normal matrix of optical flow equations
 min-feature-eig-threshold = 1e-4
 # minimum length of a feature (number of frames) to consider a feature for grouping