# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1313558713 14400
# Node ID d19d6e63dd77ca4ac469d88ea59648fea4144c57
# Parent  336926453b285df7d59f0906caf047a139628092
simple feature tracking and drawing working

diff -r 336926453b28 -r d19d6e63dd77 c/Makefile
--- a/c/Makefile	Wed Aug 17 00:20:31 2011 -0400
+++ b/c/Makefile	Wed Aug 17 01:25:13 2011 -0400
@@ -62,7 +62,7 @@
 test:
 	echo "coucou $(HOME)"
 
-feature-based-tracking: feature-based-tracking.o
+feature-based-tracking: feature-based-tracking.o cvutils.o
 	$(CXX) $(CFLAGS) $(LIBS) $^ -o $(BUILD_DIR)/$@ $(LDFLAGS)
 
 track-features.o: track-features.cpp
diff -r 336926453b28 -r d19d6e63dd77 c/cvutils.cpp
--- a/c/cvutils.cpp	Wed Aug 17 00:20:31 2011 -0400
+++ b/c/cvutils.cpp	Wed Aug 17 01:25:13 2011 -0400
@@ -10,12 +10,12 @@
 using namespace std;
 using namespace cv;
 
-void keypPoints2Points(const vector<KeyPoint>& kpts, vector<Point2f>& points) {
-  points.clear();
-  points.resize(kpts.size());
+void keyPoints2Points(const vector<KeyPoint>& kpts, vector<Point2f>& pts) {
+  pts.clear();
+  pts.reserve(kpts.size());
 
   for (unsigned int i=0; i<kpts.size(); i++)
-    points[i] = kpts[i].pt;
+    pts.push_back(kpts[i].pt);
 }
 
 IplImage* allocateImage(const int& width, const int& height, const int& depth, const int& channels) { return ::allocateImage(cvSize(width, height), depth, channels);}
diff -r 336926453b28 -r d19d6e63dd77 c/feature-based-tracking.cpp
--- a/c/feature-based-tracking.cpp	Wed Aug 17 00:20:31 2011 -0400
+++ b/c/feature-based-tracking.cpp	Wed Aug 17 01:25:13 2011 -0400
@@ -1,9 +1,11 @@
 //#include "Feature.hpp"
 #include "Parameters.hpp"
+#include "cvutils.hpp"
 #include "utils.hpp"
 
 #include "src/Trajectory.h"
 
+#include "opencv2/core/core.hpp"
 #include "opencv2/highgui/highgui.hpp"
 //#include "opencv2/imgproc/imgproc.hpp"
 #include "opencv2/features2d/features2d.hpp"
@@ -25,29 +27,48 @@
       Point2f pt_old = train[matches[i].trainIdx].pt;
       Point2f dist = pt_new - pt_old;
       if (norm(dist) < 20) {
-	cv::line(img, pt_new, pt_old, Scalar(125, 255, 125), 1);
-	cv::circle(img, pt_new, 2, Scalar(255, 0, 125), 1);
+	line(img, pt_new, pt_old, Scalar(125, 255, 125), 1);
+	circle(img, pt_old, 2, Scalar(255, 0, 125), 1);
       }
     }
 }
 
+void drawOpticalFlow(const vector<Point2f>& prevPts, const vector<Point2f>& currPts, const vector<uchar> status, Mat& img) {
+  for (unsigned int i=0; i<status.size(); i++) {
+    if (status[i]) {
+ 	line(img, prevPts[i], currPts[i], Scalar(125, 255, 125), 1);
+	circle(img, prevPts[i], 2, Scalar(255, 0, 125), 1);     
+    }
+  }
+}
+
 int main(int argc, char *argv[]) {
   //vector<TrajectoryPoint2f> features;
-
+  
   BriefDescriptorExtractor brief(32);
   const int DESIRED_FTRS = 500;
   //shared_ptr<FeatureDetector> detector = shared_ptr<FeatureDetector>(new GridAdaptedFeatureDetector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4));
-  GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
+  //GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
 
   VideoCapture capture;
-  Mat frame, display;
+  Mat frame, currentFrameBW, previousFrameBW;
 
   KLTFeatureTrackingParameters params;
   params.frame1 = 0;
   params.nFrames = -1;
-  // TODO ajouter klt paremeters, reprendre code de opencvfeaturetracker
-  //GoodFeaturesToTrackDetector detector(Params.max_nfeatures, Params.feature_quality, Params.min_feature_distance_klt, Params.window_size, Params.useHarrisDetector_GoodFeaturesToTrackDetector, Params.k_GoodFeaturesToTrackDetector);
-  // search descriptor_match.h
+  params.maxNFeatures = 1000;
+  params.featureQuality = 0.1;
+  params.minFeatureDistanceKLT = 3;
+  params.windowSize = 3; 
+  params.useHarrisDetector = false;
+  params.k = 0.4;
+  GoodFeaturesToTrackDetector detector(params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, params.windowSize, params.useHarrisDetector, params.k);
+
+  params.pyramidLevel = 3;
+  params.maxNumberTrackingIterations = 20; // 30
+  params.minTrackingError = 0.3; // 0.01
+  params.derivLambda = 0.5;
+  Size window = Size(params.windowSize, params.windowSize);
 
   BruteForceMatcher<Hamming> descMatcher;
   vector<DMatch> matches;
@@ -81,10 +102,11 @@
     }
   
   vector<KeyPoint> prevKpts, currKpts;
+  vector<Point2f> prevPts, currPts;
+  vector<uchar> status;
+  vector<float> errors;
+  Mat prevDesc, currDesc;
   
-  Mat gray;
-  
-  Mat prevDesc, currDesc;
   // TODO structure de donnee paires pointeur trajectory, numero de keypoint
   int key = '?'; 
   for (int frameNum = 0; ((params.frame1+frameNum < params.nFrames) || (params.nFrames < 0)) && !::interruptionKey(key); frameNum++) {
@@ -93,26 +115,31 @@
       while (frame.empty())
 	capture >> frame;//break;
       
-      cvtColor(frame, gray, CV_RGB2GRAY);
+      cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
       
-      detector.detect(gray, currKpts);
+      detector.detect(currentFrameBW, currKpts);
       //cout << currKpts.size() << " kpts" << endl;
       
-      brief.compute(gray, currKpts, currDesc); //Compute brief descriptors at each keypoint location
+      brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location
       
-      //display = frame.clone();
       if (!prevKpts.empty()) {
-	cout << matches.size() << " matches" << endl;
-	descMatcher.match(currDesc, prevDesc, matches);
-	cout << matches.size() << " matches" << endl;
+	::keyPoints2Points(prevKpts, prevPts);
+	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), params.derivLambda, 0); // OPTFLOW_USE_INITIAL_FLOW
+	drawOpticalFlow(prevPts, currPts, status, frame);
+
+	// cout << matches.size() << " matches" << endl;
+	// descMatcher.match(currDesc, prevDesc, matches);
+	// cout << matches.size() << " matches" << endl;
 	drawMatchesRelative(prevKpts, currKpts, matches, frame);
 	//drawMatches(frame, prevKpts, frame, currKpts, matches, display);//, Scalar::all(-1), Scalar::all(-1), vector<vector<char> >(), DrawMatchesFlags::DRAW_OVER_OUTIMG);
       }
       
       imshow("frame", frame);
+      previousFrameBW = currentFrameBW.clone();
       prevKpts = currKpts;
       currDesc.copyTo(prevDesc);
-      key = waitKey(0);
+      key = waitKey(2);
     }  
   
   return 0;
diff -r 336926453b28 -r d19d6e63dd77 include/Parameters.hpp
--- a/include/Parameters.hpp	Wed Aug 17 00:20:31 2011 -0400
+++ b/include/Parameters.hpp	Wed Aug 17 01:25:13 2011 -0400
@@ -19,6 +19,8 @@
   float featureQuality;
   float minFeatureDistanceKLT;
   int windowSize;
+  bool useHarrisDetector;
+  float k;
   int pyramidLevel;
   int nFramesDisplacement;
   float minFeatureDisplacement;
@@ -28,6 +30,7 @@
   int nFramesVelocity;
   int maxNumberTrackingIterations;
   float minTrackingError;
+  float derivLambda;
   int minFeatureTime;
   float mmConnectionDistance;
   float mmSegmentationDistance;
diff -r 336926453b28 -r d19d6e63dd77 include/cvutils.hpp
--- a/include/cvutils.hpp	Wed Aug 17 00:20:31 2011 -0400
+++ b/include/cvutils.hpp	Wed Aug 17 01:25:13 2011 -0400
@@ -10,7 +10,7 @@
 
 //static const int flipImage = CV_CVTIMG_FLIP;
 
-void keypPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& points);
+void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts);
 
 /** Allocates a new IplImage. */
 IplImage* allocateImage(const int& width, const int& height, const int& depth, const int& channels);