# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1314725911 14400
# Node ID 0089fb29cd267ce82b4abfa7f69d25814f3eafab
# Parent  7150427c665e9cc94272d776750173c03e10cedd
added projection of points and reprojection for display

diff -r 7150427c665e -r 0089fb29cd26 c/Motion.cpp
--- a/c/Motion.cpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/c/Motion.cpp	Tue Aug 30 13:38:31 2011 -0400
@@ -9,8 +9,8 @@
 using namespace std;
 using namespace cv;
 
-FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p) {
-  addPoint(frameNum, p);
+FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const Mat& homography) {
+  addPoint(frameNum, p, homography);
 }
 
 bool FeatureTrajectory::smallDisplacement(const unsigned int& nDisplacements, const float& minTotalFeatureDisplacement) const {
@@ -42,8 +42,11 @@
   return result;
 }
 
-void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
-  positions.add(frameNum, p);
+void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p, const Mat& homography) {
+  Point2f pp = p;
+  if (!homography.empty())
+    pp = project(p, homography);
+  positions.add(frameNum, pp);
   computeMotionData(frameNum);
   assert(positions.size() == displacementDistances.size()+1);
   assert(positions.size() == velocities.size()+1);
@@ -62,10 +65,17 @@
 
 #ifdef USE_OPENCV
 /// \todo add option for anti-aliased drawing, thickness
-void FeatureTrajectory::draw(Mat& img, const Scalar& color) const {
-  Point2f p1 = positions[0];
+void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const {
+  Point2f p1, p2;
+  if (!homography.empty())
+    p1 = project(positions[0], homography);
+  else
+    p1 = positions[0];
   for (unsigned int i=1; i<positions.size(); i++) {
-    Point2f p2 = positions[i];
+    if (!homography.empty())
+      p2 = project(positions[i], homography);
+    else
+      p2 = positions[i];
     line(img, p1, p2, color, 1);
     p1 = p2;
   }
diff -r 7150427c665e -r 0089fb29cd26 c/cvutils.cpp
--- a/c/cvutils.cpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/c/cvutils.cpp	Tue Aug 30 13:38:31 2011 -0400
@@ -11,7 +11,21 @@
 using namespace std;
 using namespace cv;
 
-cv::Mat loadMat(const string& filename, const string& separator) {
+Point2f project(const Point2f& p, const Mat& homography) {
+  //Mat homogeneous(3, 1, CV_32FC1);
+  float x, y;
+  float w = homography.at<float>(2,0)*p.x+homography.at<float>(2,1)*p.y+homography.at<float>(2,2);
+  if (w != 0) {
+    x = (homography.at<float>(0,0)*p.x+homography.at<float>(0,1)*p.y+homography.at<float>(0,2))/w;
+    y = (homography.at<float>(1,0)*p.x+homography.at<float>(1,1)*p.y+homography.at<float>(1,2))/w;
+  } else {
+    x = 0;
+    y = 0;
+  }
+  return Point2f(x, y);
+}
+
+Mat loadMat(const string& filename, const string& separator) {
   vector<vector<float> > numbers = ::loadNumbers(filename, separator);
   
   Mat mat;
diff -r 7150427c665e -r 0089fb29cd26 c/feature-based-tracking.cpp
--- a/c/feature-based-tracking.cpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/c/feature-based-tracking.cpp	Tue Aug 30 13:38:31 2011 -0400
@@ -53,6 +53,7 @@
 };
 
 inline void saveFeatures(vector<FeatureTrajectoryPtr>& features, TrajectoryDBAccess<Point2f>& db, const string& positionsTableName, const string& velocitiesTableName, const unsigned int& minNFeatures = 0) {
+  /// \todo smoothing
   if (features.size() >= minNFeatures) {
     BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName);
     features.clear();
@@ -70,8 +71,10 @@
   KLTFeatureTrackingParameters params(argc, argv);
   cout << params.parameterDescription << endl;
 
-  Mat m = ::loadMat(params.homographyFilename, " ");
-  //cout << m << endl;
+  Mat homography = ::loadMat(params.homographyFilename, " ");
+  Mat invHomography;
+  if (params.display && !homography.empty())
+    invHomography = homography.inv();
 
   float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
   Size window = Size(params.windowSize, params.windowSize);
@@ -119,12 +122,10 @@
   //     return 1;
   //   }
 
-  // mask
   Mat mask = imread(params.maskFilename, 0);
   if (mask.empty())
     mask = Mat::ones(videoSize, CV_8UC1);
 
-  // database
   boost::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = boost::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
   //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
   trajectoryDB->connect(params.databaseFilename.c_str());
@@ -171,7 +172,7 @@
 	  bool deleteFeature = false;
 	  
 	  if (status[iter->pointNum]) {
-	    iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
+	    iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
 
 	    deleteFeature |= iter->feature->smallDisplacement(params.nDisplacements, minTotalFeatureDisplacement)
 	      || !iter->feature->motionSmooth(params.accelerationBound, params.deviationBound);
@@ -184,8 +185,6 @@
 	    if (iter->feature->length() >= params.minFeatureTime) {
 	      iter->feature->setId(savedFeatureId);
 	      savedFeatureId++;
-	      /// \todo smoothing
-	      //iter->feature->write(*trajectoryDB);
 	      lostFeatures.push_back(iter->feature);
 	    }
 	    iter = featurePointMatches.erase(iter);
@@ -201,7 +200,7 @@
 	
 	if (params.display) {
 	  BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
-	    fp.feature->draw(frame, Colors::red());
+	    fp.feature->draw(frame, invHomography, Colors::red());
 	}
 	//drawOpticalFlow(prevPts, currPts, status, frame);
 	
@@ -219,7 +218,7 @@
 	    featureMask.at<uchar>(i,j)=0;
       goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.windowSize, 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));
+	FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
 	featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
 	currPts.push_back(p);
       }
diff -r 7150427c665e -r 0089fb29cd26 include/Motion.hpp
--- a/include/Motion.hpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/include/Motion.hpp	Tue Aug 30 13:38:31 2011 -0400
@@ -12,7 +12,7 @@
     before saving. */
 class FeatureTrajectory {
 public:
-  FeatureTrajectory(const int& frameNum, const cv::Point2f& p);
+  FeatureTrajectory(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography);
 
   unsigned int length(void) const { return positions.size();}
 
@@ -24,14 +24,14 @@
   /// 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 addPoint(const int& frameNum, const cv::Point2f& p, const cv::Mat& homography);
 
   void shorten(void);
 
   void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB, const string& positionsTableName, const string& velocitiesTableName) const;
 
 #ifdef USE_OPENCV
-  void draw(cv::Mat& img, const cv::Scalar& color) const;
+  void draw(cv::Mat& img, const cv::Mat& homography, const cv::Scalar& color) const;
 #endif
 
 protected:
diff -r 7150427c665e -r 0089fb29cd26 include/cvutils.hpp
--- a/include/cvutils.hpp	Tue Aug 30 13:04:36 2011 -0400
+++ b/include/cvutils.hpp	Tue Aug 30 13:38:31 2011 -0400
@@ -10,6 +10,10 @@
 /// constant that indicates if the image should be flipped
 //static const int flipImage = CV_CVTIMG_FLIP;
 
+/** Projects a point with the homography matrix
+ use perspectiveTransform for arrays of points. */
+cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography);
+
 /** Loads a cv mat from a text file where the numbers are saved line by line separated by separator */
 cv::Mat loadMat(const std::string& filename, const std::string& separator);