--- a/c/cvutils.cpp	Wed Aug 17 01:25:13 2011 -0400
+++ b/c/cvutils.cpp	Wed Aug 17 02:44:28 2011 -0400
@@ -10,8 +10,9 @@
 using namespace std;
 using namespace cv;
 
-void keyPoints2Points(const vector<KeyPoint>& kpts, vector<Point2f>& pts) {
-  pts.clear();
+void keyPoints2Points(const vector<KeyPoint>& kpts, vector<Point2f>& pts, const bool& clearPts /* = true */) {
+  if (clearPts)
+    pts.clear();
   pts.reserve(kpts.size());
 
   for (unsigned int i=0; i<kpts.size(); i++)

--- a/c/feature-based-tracking.cpp	Wed Aug 17 01:25:13 2011 -0400
+++ b/c/feature-based-tracking.cpp	Wed Aug 17 02:44:28 2011 -0400
@@ -72,6 +72,7 @@
 
   BruteForceMatcher<Hamming> descMatcher;
   vector<DMatch> matches;
+  Size videoSize;
 
   if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))) // if no parameter or number parameter
     capture.open(argc == 2 ? argv[1][0] - '0' : 0);
@@ -79,9 +80,10 @@
     {
       capture.open(argv[1]);
       if( capture.isOpened() )
+	videoSize = Size(capture.get(CV_CAP_PROP_FRAME_WIDTH), capture.get(CV_CAP_PROP_FRAME_HEIGHT));
 	cout << "Video " << argv[1] <<
-	  ": width=" << capture.get(CV_CAP_PROP_FRAME_WIDTH) <<
-	  ", height=" << capture.get(CV_CAP_PROP_FRAME_HEIGHT) <<
+	  ": width=" << videoSize.width <<
+	  ", height=" << videoSize.height <<
 	  ", nframes=" << capture.get(CV_CAP_PROP_FRAME_COUNT) << endl;
       if( argc > 2 && isdigit(argv[2][0]) ) // could be used to reach first frame, dumping library messages to log file (2> /tmp/log.txt)
         {
@@ -106,39 +108,54 @@
   vector<uchar> status;
   vector<float> errors;
   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++) {
+  for (int frameNum = params.frame1; ((params.frame1+frameNum < params.nFrames) || (params.nFrames < 0)) && !::interruptionKey(key); frameNum++) {
       capture >> frame;
-      cout << capture.get(CV_CAP_PROP_POS_FRAMES) << endl;
+      cout << capture.get(CV_CAP_PROP_POS_FRAMES) << " " << prevPts.size() << endl;
       while (frame.empty())
 	capture >> frame;//break;
       
       cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
       
-      detector.detect(currentFrameBW, currKpts);
-      //cout << currKpts.size() << " kpts" << endl;
-      
-      brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location
-      
-      if (!prevKpts.empty()) {
-	::keyPoints2Points(prevKpts, prevPts);
+      if (!prevPts.empty()) {
+	//::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);
-
+	
+	vector<Point2f> trackedPts;
+	for (unsigned int i=0; i<status.size(); i++)
+	  if (status[i])
+	    trackedPts.push_back(currPts[i]);
+	currPts = trackedPts;
+	
 	// 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);
+	//drawMatchesRelative(prevKpts, currKpts, matches, frame);
       }
       
+      // adding new features, using mask around existing feature positions
+      Mat featureMask = Mat::ones(videoSize, CV_8UC1);
+      for (unsigned int n=0;n<currPts.size(); n++)
+	for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
+	  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;
+      detector.detect(currentFrameBW, currKpts, featureMask);
+      ::keyPoints2Points(currKpts, currPts, false);
+
+      //brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location
+      
       imshow("frame", frame);
+      imshow("mask", featureMask*256);
       previousFrameBW = currentFrameBW.clone();
-      prevKpts = currKpts;
-      currDesc.copyTo(prevDesc);
+      prevPts = currPts;
+      //prevKpts = currKpts;
+      //currDesc.copyTo(prevDesc);
       key = waitKey(2);
     }  
   

--- a/include/cvutils.hpp	Wed Aug 17 01:25:13 2011 -0400
+++ b/include/cvutils.hpp	Wed Aug 17 02:44:28 2011 -0400
@@ -10,7 +10,7 @@
 
 //static const int flipImage = CV_CVTIMG_FLIP;
 
-void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts);
+void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts, const bool& clearPts = true);
 
 /** Allocates a new IplImage. */
 IplImage* allocateImage(const int& width, const int& height, const int& depth, const int& channels);