Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 132:45c64e68053c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added drawing function for features
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 17 Aug 2011 19:03:25 -0400
parents 2a6e7a9a5c53
children 63dd4355b6d1
comparison
equal deleted inserted replaced
131:3a11dba30655 132:45c64e68053c
9 #include "opencv2/highgui/highgui.hpp" 9 #include "opencv2/highgui/highgui.hpp"
10 //#include "opencv2/imgproc/imgproc.hpp" 10 //#include "opencv2/imgproc/imgproc.hpp"
11 #include "opencv2/features2d/features2d.hpp" 11 #include "opencv2/features2d/features2d.hpp"
12 12
13 #include <boost/shared_ptr.hpp> 13 #include <boost/shared_ptr.hpp>
14 #include <boost/foreach.hpp>
14 15
15 #include <iostream> 16 #include <iostream>
16 //#include <list> 17 //#include <list>
17 #include <vector> 18 #include <vector>
18 19
19 using namespace std; 20 using namespace std;
20 using namespace cv; 21 using namespace cv;
21 using namespace boost;
22 22
23 void drawMatchesRelative(const vector<KeyPoint>& train, const vector<KeyPoint>& query, std::vector<cv::DMatch>& matches, Mat& img) { 23 void drawMatchesRelative(const vector<KeyPoint>& train, const vector<KeyPoint>& query, std::vector<cv::DMatch>& matches, Mat& img) {
24 for (int i = 0; i < (int)matches.size(); i++) 24 for (int i = 0; i < (int)matches.size(); i++)
25 { 25 {
26 Point2f pt_new = query[matches[i].queryIdx].pt; 26 Point2f pt_new = query[matches[i].queryIdx].pt;
40 circle(img, prevPts[i], 2, Scalar(255, 0, 125), 1); 40 circle(img, prevPts[i], 2, Scalar(255, 0, 125), 1);
41 } 41 }
42 } 42 }
43 } 43 }
44 44
45 struct FeaturePointMatch {
46 FeatureTrajectoryPtr feature;
47 int pointNum;
48
49 FeaturePointMatch(FeatureTrajectoryPtr _feature, const int& _pointNum):
50 feature(_feature), pointNum(_pointNum) {}
51 };
52
45 int main(int argc, char *argv[]) { 53 int main(int argc, char *argv[]) {
46 BriefDescriptorExtractor brief(32); 54 // BriefDescriptorExtractor brief(32);
47 const int DESIRED_FTRS = 500; 55 // const int DESIRED_FTRS = 500;
48 //shared_ptr<FeatureDetector> detector = shared_ptr<FeatureDetector>(new GridAdaptedFeatureDetector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4)); 56 // GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
49 //GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
50 57
51 VideoCapture capture; 58 VideoCapture capture;
52 Mat frame, currentFrameBW, previousFrameBW; 59 Mat frame, currentFrameBW, previousFrameBW;
53 60
54 KLTFeatureTrackingParameters params; 61 KLTFeatureTrackingParameters params;
105 vector<Point2f> prevPts, currPts, newPts; 112 vector<Point2f> prevPts, currPts, newPts;
106 vector<uchar> status; 113 vector<uchar> status;
107 vector<float> errors; 114 vector<float> errors;
108 Mat prevDesc, currDesc; 115 Mat prevDesc, currDesc;
109 116
110 vector<FeatureTrajectory> features; 117 vector<FeatureTrajectoryPtr> features;
118 vector<FeaturePointMatch> featurePointMatches;
111 119
112 // TODO structure de donnee paires pointeur trajectory, numero de keypoint 120 // TODO structure de donnee paires pointeur trajectory, numero de keypoint
113 int key = '?'; 121 int key = '?';
114 for (int frameNum = params.frame1; ((params.frame1+frameNum < params.nFrames) || (params.nFrames < 0)) && !::interruptionKey(key); frameNum++) { 122 for (int frameNum = params.frame1; ((params.frame1+frameNum < params.nFrames) || (params.nFrames < 0)) && !::interruptionKey(key); frameNum++) {
115 capture >> frame; 123 capture >> frame;
125 if (!prevPts.empty()) { 133 if (!prevPts.empty()) {
126 //::keyPoints2Points(prevKpts, prevPts); 134 //::keyPoints2Points(prevKpts, prevPts);
127 currPts.clear(); 135 currPts.clear();
128 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 136 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
129 137
130 drawOpticalFlow(prevPts, currPts, status, frame);
131
132 vector<Point2f> trackedPts; 138 vector<Point2f> trackedPts;
133 for (unsigned int i=0; i<status.size(); i++) 139 vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
134 if (status[i]) 140 while (iter != featurePointMatches.end()) {
135 trackedPts.push_back(currPts[i]); 141 if (status[iter->pointNum]) {
142 iter->feature->addPoint(frameNum, currPts[iter->pointNum]);
143 trackedPts.push_back(currPts[iter->pointNum]);
144 iter->pointNum = trackedPts.size()-1;
145 iter++;
146 } else {
147 // save feature
148 iter = featurePointMatches.erase(iter);
149 }
150 }
136 currPts = trackedPts; 151 currPts = trackedPts;
137 152 assert(currPts.size() == featurePointMatches.size());
153
154 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
155 fp.feature->draw(frame, Colors::red());
156 //drawOpticalFlow(prevPts, currPts, status, frame);
157
138 // cout << matches.size() << " matches" << endl; 158 // cout << matches.size() << " matches" << endl;
139 // descMatcher.match(currDesc, prevDesc, matches); 159 // descMatcher.match(currDesc, prevDesc, matches);
140 // cout << matches.size() << " matches" << endl; 160 // cout << matches.size() << " matches" << endl;
141 //drawMatchesRelative(prevKpts, currKpts, matches, frame); 161 //drawMatchesRelative(prevKpts, currKpts, matches, frame);
142 } 162 }
146 for (unsigned int n=0;n<currPts.size(); n++) 166 for (unsigned int n=0;n<currPts.size(); n++)
147 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 167 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
148 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) 168 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
149 featureMask.at<uchar>(i,j)=0; 169 featureMask.at<uchar>(i,j)=0;
150 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.windowSize, params.useHarrisDetector, params.k); 170 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.windowSize, params.useHarrisDetector, params.k);
151 currPts.insert(currPts.end(), newPts.begin(), newPts.end()); 171 BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
172 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p));
173 //features.push_back(f);
174 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
175 currPts.push_back(p);
176 }
177 // currPts.insert(currPts.end(), newPts.begin(), newPts.end());
152 //::keyPoints2Points(currKpts, currPts, false); 178 //::keyPoints2Points(currKpts, currPts, false);
153 179
154 //brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location 180 //brief.compute(currentFrameBW, currKpts, currDesc); //Compute brief descriptors at each keypoint location
155 181
156 imshow("frame", frame); 182 imshow("frame", frame);
157 imshow("mask", featureMask*256); 183 imshow("mask", featureMask*256);
158 previousFrameBW = currentFrameBW.clone(); 184 previousFrameBW = currentFrameBW.clone();
159 prevPts = currPts; 185 prevPts = currPts;
160 //prevKpts = currKpts; 186 //prevKpts = currKpts;
161 //currDesc.copyTo(prevDesc); 187 //currDesc.copyTo(prevDesc);
162 key = waitKey(2); 188 key = waitKey(0);
163 } 189 }
164 190
165 return 0; 191 return 0;
166 } 192 }
167 193