# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1398980470 14400
# Node ID 081a9da6f85b888c49ad29a5dc44015a0b0a2868
# Parent  13d4eb96a7512c741e17ce5e20f98ba3c2b6e1c1# Parent  b96ff16b1c81e570d5069e12f3d52e95e233bfa5
first version with undistort implemented in the feature tracking process

diff -r b96ff16b1c81 -r 081a9da6f85b c/Parameters.cpp
--- a/c/Parameters.cpp	Thu May 01 11:08:41 2014 -0400
+++ b/c/Parameters.cpp	Thu May 01 17:41:10 2014 -0400
@@ -29,7 +29,11 @@
     ("video-filename", po::value<string>(&videoFilename), "filename of the video to process")
     ("database-filename", po::value<string>(&databaseFilename), "filename of the database where results are saved")
     ("homography-filename", po::value<string>(&homographyFilename), "filename of the homography matrix")
+    ("intrinsic-camera-filename", po::value<string>(&intrinsicCameraFilename), "filename of the homography matrix")
+    ("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")
@@ -126,7 +130,10 @@
       stream << boost::any_cast<float>(value) << separator;
     else if (value.type() == typeid(string))
       stream << boost::any_cast<string>(value) << separator;
-    else
+    else if (value.type() == typeid(vector<float>)) {
+      for (unsigned int j=0; j<boost::any_cast<vector<float> >(value).size(); j++)
+	     stream << boost::any_cast<vector<float> >(value)[j] << separator;
+    } else
       cerr << "the type of the option " << optionsVec[i]->long_name() << " (" << i << ") is not int, float or string." << endl;
   }
 
diff -r b96ff16b1c81 -r 081a9da6f85b c/feature-based-tracking.cpp
--- a/c/feature-based-tracking.cpp	Thu May 01 11:08:41 2014 -0400
+++ b/c/feature-based-tracking.cpp	Thu May 01 17:41:10 2014 -0400
@@ -59,7 +59,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,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);
@@ -94,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>());
@@ -122,12 +138,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 +149,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, INTER_LINEAR, BORDER_CONSTANT, 0.);
+	frame = undistortedFrame;
+      }
+      //if (!success || frame.empty() || frame.size() != videoSize)
+      //break;
 
       if (frameNum%50 ==0)
 	cout << "frame " << frameNum << endl;
diff -r b96ff16b1c81 -r 081a9da6f85b include/Parameters.hpp
--- a/include/Parameters.hpp	Thu May 01 11:08:41 2014 -0400
+++ b/include/Parameters.hpp	Thu May 01 17:41:10 2014 -0400
@@ -4,6 +4,7 @@
 /// \todo Class for parameters, with utilities to save and load from configuration files
 
 #include <string>
+#include <vector>
 
 namespace boost{
   namespace program_options {
@@ -19,7 +20,11 @@
   std::string videoFilename;
   std::string databaseFilename;
   std::string homographyFilename;
+  std::string intrinsicCameraFilename;
+  std::vector<float> distortionCoefficients;
+  float undistortedImageMultiplication;
   std::string maskFilename;
+  bool undistort;
   bool loadFeatures;
   bool display;
   float videoFPS;
diff -r b96ff16b1c81 -r 081a9da6f85b tracking.cfg
--- a/tracking.cfg	Thu May 01 11:08:41 2014 -0400
+++ b/tracking.cfg	Thu May 01 17:41:10 2014 -0400
@@ -4,6 +4,16 @@
 database-filename = laurier.sqlite
 # filename of the homography matrix
 homography-filename = laurier-homography.txt
+# filename of the homography matrix
+intrinsic-camera-filename = intrinsic-camera.txt
+# -0.11759321 0.0148536 0.00030756 -0.00020578 -0.00091816
+distortion-coefficients = -0.11759321
+distortion-coefficients = 0.0148536
+distortion-coefficients = 0.00030756 
+distortion-coefficients = -0.00020578 
+distortion-coefficients = -0.00091816
+# undistorted image multiplication
+undistorted-size-multiplication = 1.31
 # filename of the mask image (where features are detected)
 mask-filename = none
 # load features from database