--- a/c/feature-based-tracking.cpp	Wed Aug 17 19:03:11 2011 -0400
+++ b/c/feature-based-tracking.cpp	Wed Aug 17 19:03:25 2011 -0400
@@ -11,6 +11,7 @@
 #include "opencv2/features2d/features2d.hpp"
 
 #include <boost/shared_ptr.hpp>
+#include <boost/foreach.hpp>
 
 #include <iostream>
 //#include <list>
@@ -18,7 +19,6 @@
 
 using namespace std;
 using namespace cv;
-using namespace boost;
 
 void drawMatchesRelative(const vector<KeyPoint>& train, const vector<KeyPoint>& query, std::vector<cv::DMatch>& matches, Mat& img) {
   for (int i = 0; i < (int)matches.size(); i++)
@@ -42,11 +42,18 @@
   }
 }
 
+struct FeaturePointMatch {
+  FeatureTrajectoryPtr feature;
+  int pointNum;
+
+  FeaturePointMatch(FeatureTrajectoryPtr _feature, const int& _pointNum):
+    feature(_feature), pointNum(_pointNum) {}
+};
+
 int main(int argc, char *argv[]) {
-  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);
+  // BriefDescriptorExtractor brief(32);
+  // const int DESIRED_FTRS = 500;
+  // GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
 
   VideoCapture capture;
   Mat frame, currentFrameBW, previousFrameBW;
@@ -107,7 +114,8 @@
   vector<float> errors;
   Mat prevDesc, currDesc;
 
-  vector<FeatureTrajectory> features;
+  vector<FeatureTrajectoryPtr> features;
+  vector<FeaturePointMatch> featurePointMatches;
     
   // TODO structure de donnee paires pointeur trajectory, numero de keypoint
   int key = '?'; 
@@ -127,14 +135,26 @@
 	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);
-	
 	vector<Point2f> trackedPts;
-	for (unsigned int i=0; i<status.size(); i++)
-	  if (status[i])
-	    trackedPts.push_back(currPts[i]);
+	vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
+	while (iter != featurePointMatches.end()) {
+	  if (status[iter->pointNum]) {
+	    iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
+	    trackedPts.push_back(currPts[iter->pointNum]);
+	    iter->pointNum = trackedPts.size()-1;
+	    iter++;
+	  } else {
+	    // save feature
+	    iter = featurePointMatches.erase(iter);
+	  }
+	}
 	currPts = trackedPts;
-	
+	assert(currPts.size() == featurePointMatches.size());
+
+	BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
+	  fp.feature->draw(frame, Colors::red());
+	//drawOpticalFlow(prevPts, currPts, status, frame);
+		
 	// cout << matches.size() << " matches" << endl;
 	// descMatcher.match(currDesc, prevDesc, matches);
 	// cout << matches.size() << " matches" << endl;
@@ -148,7 +168,13 @@
 	  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);
-      currPts.insert(currPts.end(), newPts.begin(), newPts.end());
+      BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
+	FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p));
+	//features.push_back(f);
+	featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
+	currPts.push_back(p);
+      }
+      // currPts.insert(currPts.end(), newPts.begin(), newPts.end());
       //::keyPoints2Points(currKpts, currPts, false);
 
       //brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location
@@ -159,7 +185,7 @@
       prevPts = currPts;
       //prevKpts = currKpts;
       //currDesc.copyTo(prevDesc);
-      key = waitKey(2);
+      key = waitKey(0);
     }  
   
   return 0;