--- 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()));