Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 655:39fa1c998b29

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
removed the abstract class to represent folders of images or video files since the capability is now built in OpenCV
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Thu, 07 May 2015 16:30:58 +0200
parents 045d05cef9d0
children 576d9ea4b41a
comparison
equal deleted inserted replaced
654:045d05cef9d0 655:39fa1c998b29
1 #include "Motion.hpp" 1 #include "Motion.hpp"
2 #include "Parameters.hpp" 2 #include "Parameters.hpp"
3 #include "cvutils.hpp" 3 #include "cvutils.hpp"
4 #include "utils.hpp" 4 #include "utils.hpp"
5 #include "InputVideoFileModule.h"
6 #include "InputFrameListModule.h"
7 5
8 #include "src/Trajectory.h" 6 #include "src/Trajectory.h"
9 #include "src/TrajectoryDBAccessList.h" 7 #include "src/TrajectoryDBAccessList.h"
10 #include "src/TrajectoryDBAccessBlob.h" 8 #include "src/TrajectoryDBAccessBlob.h"
11 9
15 #include "opencv2/features2d/features2d.hpp" 13 #include "opencv2/features2d/features2d.hpp"
16 #include "opencv2/highgui/highgui.hpp" 14 #include "opencv2/highgui/highgui.hpp"
17 #include "opencv2/objdetect/objdetect.hpp" 15 #include "opencv2/objdetect/objdetect.hpp"
18 16
19 #include <boost/foreach.hpp> 17 #include <boost/foreach.hpp>
20 #include <boost/filesystem.hpp>
21 18
22 #include <iostream> 19 #include <iostream>
23 #include <vector> 20 #include <vector>
24 #include <ctime> 21 #include <ctime>
25 #include <cmath> 22 #include <cmath>
26 #include <memory> 23 #include <memory>
27 24
28 using namespace std; 25 using namespace std;
29 using namespace cv; 26 using namespace cv;
30 namespace fs = boost::filesystem;
31 27
32 void drawMatchesRelative(const vector<KeyPoint>& train, const vector<KeyPoint>& query, std::vector<cv::DMatch>& matches, Mat& img) { 28 void drawMatchesRelative(const vector<KeyPoint>& train, const vector<KeyPoint>& query, std::vector<cv::DMatch>& matches, Mat& img) {
33 for (int i = 0; i < (int)matches.size(); i++) 29 for (int i = 0; i < (int)matches.size(); i++)
34 { 30 {
35 Point2f pt_new = query[matches[i].queryIdx].pt; 31 Point2f pt_new = query[matches[i].queryIdx].pt;
90 cout << "Unsupported option " << interpolationMethod << " for interpolation method" << endl; 86 cout << "Unsupported option " << interpolationMethod << " for interpolation method" << endl;
91 87
92 // BruteForceMatcher<Hamming> descMatcher; 88 // BruteForceMatcher<Hamming> descMatcher;
93 // vector<DMatch> matches; 89 // vector<DMatch> matches;
94 90
95 std::shared_ptr<InputFrameProviderIface> capture; 91 if(params.videoFilename.empty()) {
96 if (fs::is_directory(fs::path(params.videoFilename))) 92 cout << "Empty video filename. Exiting." << endl;
97 capture = std::shared_ptr<InputFrameListModule>(new InputFrameListModule(params.videoFilename)); 93 exit(0);
98 else if(!params.videoFilename.empty()) 94 }
99 capture = std::shared_ptr<InputVideoFileModule>(new InputVideoFileModule(params.videoFilename)); 95
100 else 96 VideoCapture capture(params.videoFilename);
101 cout << "No valid input parameters" << endl; 97 if(!capture.isOpened()) {
102
103 if(!capture->isOpen()) {
104 cout << "Video filename " << params.videoFilename << " could not be opened. Exiting." << endl; 98 cout << "Video filename " << params.videoFilename << " could not be opened. Exiting." << endl;
105 exit(0); 99 exit(0);
106 } 100 }
107 101
108 Size videoSize = capture->getSize(); 102 Size videoSize = Size(capture.get(CV_CAP_PROP_FRAME_WIDTH), capture.get(CV_CAP_PROP_FRAME_HEIGHT));
109 unsigned int nFrames = capture->getNbFrames(); 103 unsigned int nFrames = capture.get(CV_CAP_PROP_FRAME_COUNT);
110 cout << "Video " << params.videoFilename << 104 cout << "Video " << params.videoFilename <<
111 ": width=" << videoSize.width << 105 ": width=" << videoSize.width <<
112 ", height=" << videoSize.height << 106 ", height=" << videoSize.height <<
113 ", nframes=" << nFrames << endl; 107 ", nframes=" << nFrames << endl;
114 108
153 147
154 unsigned int lastFrameNum = nFrames; 148 unsigned int lastFrameNum = nFrames;
155 if (params.nFrames > 0) 149 if (params.nFrames > 0)
156 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); 150 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
157 151
158 capture->setFrameNumber(params.frame1); 152 capture.set(CV_CAP_PROP_POS_FRAMES, params.frame1);
159 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) { 153 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
160 bool success = capture->getNextFrame(frame); 154 capture >> frame;
161 if (!success || frame.empty()) { 155 if (frame.empty()) {
162 cout << "Empty frame " << frameNum << ", breaking (" << success << " " << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; 156 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl;
157 break;
158 } else if (frameNum%50 ==0)
159 cout << "frame " << frameNum << endl;
160
161 if (params.undistort) {
162 remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
163 frame = undistortedFrame;
164
165 if (frame.size() != videoSize) {
166 cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl;
163 break; 167 break;
164 } else if (frameNum%50 ==0) 168 }
165 cout << "frame " << frameNum << endl; 169 }
166 170
167 if (params.undistort) { 171
168 remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); 172 cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
169 frame = undistortedFrame; 173
170 174 if (!prevPts.empty()) {
171 if (frame.size() != videoSize) { 175 currPts.clear();
172 cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; 176 calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
173 break; 177 /// \todo try calcOpticalFlowFarneback
178
179 std::vector<Point2f> trackedPts;
180 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
181 while (iter != featurePointMatches.end()) {
182 bool deleteFeature = false;
183
184 if (status[iter->pointNum]) {
185 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
186
187 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
188 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
189 if (deleteFeature)
190 iter->feature->shorten();
191 } else
192 deleteFeature = true;
193
194 if (deleteFeature) {
195 if (iter->feature->length() >= params.minFeatureTime) {
196 iter->feature->setId(savedFeatureId);
197 savedFeatureId++;
198 iter->feature->movingAverage(params.nFramesSmoothing);
199 lostFeatures.push_back(iter->feature);
200 }
201 iter = featurePointMatches.erase(iter);
202 } else {
203 trackedPts.push_back(currPts[iter->pointNum]);
204 iter->pointNum = trackedPts.size()-1;
205 iter++;
174 } 206 }
175 } 207 }
176 208 currPts = trackedPts;
209 assert(currPts.size() == featurePointMatches.size());
210 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
211
212 if (params.display) {
213 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
214 fp.feature->draw(frame, invHomography, Colors::red());
215 // object detection
216 // vector<Rect> locations;
217 // hog.detectMultiScale(frame, locations, 0, Size(8,8), Size(32,32), 1.05, 2);
218 // BOOST_FOREACH(Rect r, locations)
219 // rectangle(frame, r.tl(), r.br(), cv::Scalar(0,255,0), 3);
220 }
221 }
177 222
178 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); 223 // adding new features, using mask around existing feature positions
224 Mat featureMask = mask.clone();
225 for (unsigned int n=0;n<currPts.size(); n++)
226 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
227 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
228 featureMask.at<uchar>(i,j)=0;
229 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
230 BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
231 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
232 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
233 currPts.push_back(p);
234 }
179 235
180 if (!prevPts.empty()) { 236 if (params.display) {
181 currPts.clear(); 237 imshow("mask", featureMask*256);
182 calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold); 238 imshow("frame", frame);
183 /// \todo try calcOpticalFlowFarneback 239 key = waitKey(2);
184 240 }
185 std::vector<Point2f> trackedPts; 241 previousFrameBW = currentFrameBW.clone();
186 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 242 prevPts = currPts;
187 while (iter != featurePointMatches.end()) {
188 bool deleteFeature = false;
189
190 if (status[iter->pointNum]) {
191 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
192
193 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
194 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
195 if (deleteFeature)
196 iter->feature->shorten();
197 } else
198 deleteFeature = true;
199
200 if (deleteFeature) {
201 if (iter->feature->length() >= params.minFeatureTime) {
202 iter->feature->setId(savedFeatureId);
203 savedFeatureId++;
204 iter->feature->movingAverage(params.nFramesSmoothing);
205 lostFeatures.push_back(iter->feature);
206 }
207 iter = featurePointMatches.erase(iter);
208 } else {
209 trackedPts.push_back(currPts[iter->pointNum]);
210 iter->pointNum = trackedPts.size()-1;
211 iter++;
212 }
213 }
214 currPts = trackedPts;
215 assert(currPts.size() == featurePointMatches.size());
216 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
217
218 if (params.display) {
219 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
220 fp.feature->draw(frame, invHomography, Colors::red());
221 // object detection
222 // vector<Rect> locations;
223 // hog.detectMultiScale(frame, locations, 0, Size(8,8), Size(32,32), 1.05, 2);
224 // BOOST_FOREACH(Rect r, locations)
225 // rectangle(frame, r.tl(), r.br(), cv::Scalar(0,255,0), 3);
226 }
227 }
228
229 // adding new features, using mask around existing feature positions
230 Mat featureMask = mask.clone();
231 for (unsigned int n=0;n<currPts.size(); n++)
232 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
233 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
234 featureMask.at<uchar>(i,j)=0;
235 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
236 BOOST_FOREACH(Point2f p, newPts) { //for (unsigned int i=0; i<newPts.size(); i++) {
237 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography));
238 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
239 currPts.push_back(p);
240 }
241
242 if (params.display) {
243 imshow("mask", featureMask*256);
244 imshow("frame", frame);
245 key = waitKey(2);
246 }
247 previousFrameBW = currentFrameBW.clone();
248 prevPts = currPts;
249 } 243 }
250 244
251 // save the remaining currently tracked features 245 // save the remaining currently tracked features
252 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 246 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
253 while (iter != featurePointMatches.end()) { 247 while (iter != featurePointMatches.end()) {