--- a/c/feature-based-tracking.cpp	Wed Jun 04 10:56:12 2014 -0400
+++ b/c/feature-based-tracking.cpp	Wed Jun 04 10:57:09 2014 -0400
@@ -23,6 +23,7 @@
 #include <iostream>
 #include <vector>
 #include <ctime>
+#include <cmath>
 
 using namespace std;
 using namespace cv;
@@ -59,7 +60,6 @@
 };
 
 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName) {
-  /// \todo smoothing
   BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName);
   features.clear();
 }
@@ -73,10 +73,24 @@
   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);
 
+  int interpolationMethod = -1;
+  if (params.interpolationMethod == 0)
+    interpolationMethod = INTER_NEAREST;
+  else if (params.interpolationMethod == 1)
+    interpolationMethod = INTER_LINEAR;
+  else if (params.interpolationMethod == 2)
+    interpolationMethod = INTER_CUBIC;
+  else if (params.interpolationMethod == 3)
+    interpolationMethod = INTER_LANCZOS4;
+  else
+    cout << "Unsupported option " << interpolationMethod << " for interpolation method" << endl;
+
   // BruteForceMatcher<Hamming> descMatcher;
   // vector<DMatch> matches;
 
@@ -99,6 +113,18 @@
 	  ": width=" << videoSize.width <<
 	  ", height=" << videoSize.height <<
 	  ", nframes=" << nFrames << endl;
+
+  Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); 
+  Mat map1, map2;
+  if (params.undistort) {
+    videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
+    newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.;
+    newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.;
+    initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2);
+    
+    cout << "Undistorted width=" << videoSize.width <<
+      ", height=" << videoSize.height << endl;
+  }
   
   Mat mask = imread(params.maskFilename, 0);
   if (mask.empty()) {
@@ -122,12 +148,9 @@
   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)
@@ -136,9 +159,12 @@
   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, interpolationMethod, BORDER_CONSTANT, 0.);
+	frame = undistortedFrame;
+      }
+      //if (!success || frame.empty() || frame.size() != videoSize)
+      //break;
 
       if (frameNum%50 ==0)
 	cout << "frame " << frameNum << endl;