Mercurial Hosting > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 804:17e54690af8a dev

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
work in progress, not fully functional yet
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Wed, 01 Jun 2016 17:57:49 -0400
parents f7cf43b5ad3b
children
comparison
equal deleted inserted replaced
803:f7cf43b5ad3b 804:17e54690af8a
62 features.clear(); 62 features.clear();
63 } 63 }
64 64
65 void trackFeatures(const KLTFeatureTrackingParameters& params) { 65 void trackFeatures(const KLTFeatureTrackingParameters& params) {
66 Mat refHomography = ::loadMat(params.homographyFilename, " "); 66 Mat refHomography = ::loadMat(params.homographyFilename, " ");
67 Mat stabilizationHomography, homography = refHomography; 67 //if (params.stabilize && refHomography.empty())
68 if (params.stabilize && refHomography.empty()) 68 // refHomography = Mat::eye(3, 3, CV_64FC1);
69 refHomography = Mat::eye(3, 3, CV_32FC1); 69 //Mat stabilizationHomography(3,3,CV_64FC1), homography(3,3,CV_64FC1);
70 //Mat invHomography; 70 Mat stabilizationHomography, homography;
71 //if (params.display && !homography.empty()) 71 if (!params.stabilize)
72 // invHomography = homography.inv(); 72 homography = refHomography;
73 73
74 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; 74 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement;
75 Size window = Size(params.windowSize, params.windowSize); 75 Size window = Size(params.windowSize, params.windowSize);
76 76
77 int interpolationMethod = -1; 77 int interpolationMethod = -1;
78 if (params.interpolationMethod == 0) 78 if (params.interpolationMethod == 0)
129 Mat mask = imread(params.maskFilename, 0); 129 Mat mask = imread(params.maskFilename, 0);
130 if (mask.empty()) { 130 if (mask.empty()) {
131 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; 131 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
132 mask = Mat::ones(videoSize, CV_8UC1); 132 mask = Mat::ones(videoSize, CV_8UC1);
133 } 133 }
134 Mat featureMask = mask.clone();
134 135
135 std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>()); 136 std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
136 //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>(); 137 //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
137 trajectoryDB->connect(params.databaseFilename.c_str()); 138 trajectoryDB->connect(params.databaseFilename.c_str());
138 trajectoryDB->createTable("positions"); 139 trajectoryDB->createTable("positions");
139 trajectoryDB->createTable("velocities"); 140 trajectoryDB->createTable("velocities");
140 trajectoryDB->beginTransaction(); 141 trajectoryDB->beginTransaction();
141 142
142 std::vector<KeyPoint> prevKpts, currKpts; 143 std::vector<KeyPoint> prevKpts, currKpts;
143 std::vector<Point2f> prevPts, currPts, newPts, homographyRefPts, homographyCurrPts; 144 std::vector<Point2f> prevPts, currPts, newPts, stabilizationRefPts, stabilizationCurrPts;
144 std::vector<uchar> status, homographPtsStatus; 145 std::vector<uchar> status, stabilizationPtsStatus;
145 std::vector<float> errors, homographyErrors; 146 std::vector<float> errorPts, stabilizationErrorPts;
146 Mat prevDesc, currDesc; 147 Mat prevDesc, currDesc;
147 148
148 std::vector<FeatureTrajectoryPtr> lostFeatures; 149 std::vector<FeatureTrajectoryPtr> lostFeatures;
149 std::vector<FeaturePointMatch> featurePointMatches; 150 std::vector<FeaturePointMatch> featurePointMatches;
150 151
161 remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); 162 remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
162 homographyFrame = undistortedFrame; 163 homographyFrame = undistortedFrame;
163 } 164 }
164 165
165 cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY); 166 cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY);
166 goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? 167 goodFeaturesToTrack(homographyFrameBW, stabilizationRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features?
167 } 168 }
168 } 169 }
169 170
170 unsigned int lastFrameNum = nFrames; 171 unsigned int lastFrameNum = nFrames;
171 if (params.nFrames > 0) 172 if (params.nFrames > 0)
172 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); 173 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
173 174
175 // Main Loop
174 capture.set(CV_CAP_PROP_POS_FRAMES, params.frame1); 176 capture.set(CV_CAP_PROP_POS_FRAMES, params.frame1);
175 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) { 177 for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) {
176 capture >> frame; 178 capture >> frame;
177 if (frame.empty()) { 179 if (frame.empty()) {
178 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; 180 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl;
191 } 193 }
192 } 194 }
193 195
194 if (!prevPts.empty()) { 196 if (!prevPts.empty()) {
195 currPts.clear(); 197 currPts.clear();
196 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); 198 calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errorPts, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
197 /// \todo try calcOpticalFlowFarneback 199 /// \todo try calcOpticalFlowFarneback
198 200
199 if (params.stabilize) { 201 if (params.stabilize) {
200 calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, homographyRefPts, homographyCurrPts, homographPtsStatus, homographyErrors, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold); 202 calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, stabilizationRefPts, stabilizationCurrPts, stabilizationPtsStatus, stabilizationErrorPts, window, params.pyramidLevel, TermCriteria(static_cast<int>(TermCriteria::COUNT)+static_cast<int>(TermCriteria::EPS) /* = 3 */, params.maxNumberTrackingIterations, params.minTrackingError), /* int flags = */ 0, params.minFeatureEigThreshold);
201 203
202 std::vector<Point2f> homographyRefPts2, homographyCurrPts2; 204 std::vector<Point2f> stabilizationRefPts2, stabilizationCurrPts2;
203 for (unsigned int i=0; i<homographyRefPts.size(); i++) 205 for (unsigned int i=0; i<stabilizationRefPts.size(); i++)
204 if (homographPtsStatus[i]) { 206 if (stabilizationPtsStatus[i]) {
205 homographyRefPts2.push_back(homographyRefPts[i]); 207 stabilizationRefPts2.push_back(stabilizationRefPts[i]);
206 homographyCurrPts2.push_back(homographyCurrPts[i]); 208 stabilizationCurrPts2.push_back(stabilizationCurrPts[i]);
207 } 209 }
208 stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold); 210 stabilizationHomography = findHomography(stabilizationCurrPts2, stabilizationRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold);
209 if (stabilizationHomography.empty()) 211 //cout << stabilizationHomography << endl;
212 //cout << stabilizationHomography.type() << " " << refHomography.type() << endl;
213 //for (unsigned int i=0; i<MIN(stabilizationRefPts.size(), 10); i++) {
214 //cout << stabilizationCurrPts2[i] << " " << stabilizationRefPts2[i] << ": projected " << ::project<double>(stabilizationCurrPts2[i], stabilizationHomography) << endl;
215 //cout << stabilizationHomography.at<double>(0,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(0,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(0,2) << " " << stabilizationHomography.at<double>(1,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(1,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(1,2) << " " << stabilizationHomography.at<double>(2,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(2,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(2,2) << endl;
216 //}
217 if (stabilizationHomography.empty()) {
210 homography = refHomography; 218 homography = refHomography;
211 else 219 featureMask = mask.clone();
212 homography = stabilizationHomography*refHomography; 220 } else {
213 } 221 if (refHomography.empty())
222 homography = stabilizationHomography;
223 else
224 homography = refHomography*stabilizationHomography;
225 //cout << refHomography << " * " << stabilizationHomography << " = " << homography << endl;
226 warpPerspective(mask, featureMask, stabilizationHomography, videoSize, INTER_LINEAR+WARP_INVERSE_MAP, BORDER_CONSTANT, 0);
227 }
228 } else
229 featureMask = mask.clone();
230
214 231
215 std::vector<Point2f> trackedPts; 232 std::vector<Point2f> trackedPts;
216 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 233 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
217 while (iter != featurePointMatches.end()) { 234 while (iter != featurePointMatches.end()) {
218 bool deleteFeature = false; 235 bool deleteFeature = false;
219 236
220 if (status[iter->pointNum] && (mask.at<uchar>(static_cast<int>(round(currPts[iter->pointNum].y)), static_cast<int>(round(currPts[iter->pointNum].x))) != 0)) { 237 if (status[iter->pointNum]) {
221 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); 238 Point2f homographyFramePt = ::project<double>(currPts[iter->pointNum], stabilizationHomography); // project to space of homography frame (if not stabilization, stabilizationHomography should be empty)
222 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) 239 cout << currPts[iter->pointNum] << ", " << homographyFramePt << endl;
223 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); 240 if (::inImage(homographyFramePt, videoSize) && (mask.at<uchar>(static_cast<int>(round(homographyFramePt.y)), static_cast<int>(round(homographyFramePt.x))) != 0)) { // check point is in mask
224 if (deleteFeature) 241 iter->feature->addPoint(frameNum, ::project<double>(homographyFramePt, refHomography));
225 iter->feature->shorten(); 242 //cout << *(iter->feature) << endl;
243 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
244 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
245 if (deleteFeature)
246 iter->feature->shorten();
247 } else
248 deleteFeature = true;
226 } else 249 } else
227 deleteFeature = true; 250 deleteFeature = true;
228 251
229 if (deleteFeature) { 252 if (deleteFeature) {
230 if (iter->feature->length() >= params.minFeatureTime) { 253 if (iter->feature->length() >= params.minFeatureTime) {
243 currPts = trackedPts; 266 currPts = trackedPts;
244 assert(currPts.size() == featurePointMatches.size()); 267 assert(currPts.size() == featurePointMatches.size());
245 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); 268 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
246 269
247 if (params.display) { 270 if (params.display) {
248 cout << featurePointMatches.size() << endl; 271 cout << featurePointMatches.size() << " matches" << endl;
272 Mat invHomography;
273 if (!homography.empty())
274 invHomography = homography.inv();
249 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) 275 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
250 fp.feature->draw(frame, homography.inv(), Colors::red()); 276 fp.feature->draw(frame, invHomography, Colors::red());
251 } 277 }
252 } 278 }
253 279
254 // adding new features, using mask around existing feature positions 280 // adding new features, using mask around existing feature positions
255 Mat featureMask = mask.clone();
256 for (unsigned int n=0;n<currPts.size(); n++) 281 for (unsigned int n=0;n<currPts.size(); n++)
257 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 282 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
258 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) 283 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
259 featureMask.at<uchar>(i,j)=0; 284 featureMask.at<uchar>(i,j)=0;
260 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k); 285 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
261 BOOST_FOREACH(Point2f p, newPts) { 286 BOOST_FOREACH(Point2f p, newPts) {
262 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography)); 287 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, ::project<double>(::project<double>(p, stabilizationHomography), refHomography)));
263 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); 288 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
264 currPts.push_back(p); 289 currPts.push_back(p);
265 } 290 }
266 291
267 if (params.display) { 292 if (params.display) {
268 imshow("mask", featureMask*256); 293 cout << currPts.size() << " current points" << endl;
294 Mat maskedFrame;
295 cvtColor(featureMask*256, maskedFrame, CV_GRAY2RGB);
296 addWeighted(frame, 0.5, maskedFrame, 0.5, 0.0, maskedFrame);
297 imshow("masked frame", maskedFrame);
298 //imshow("mask", featureMask*256);
269 imshow("frame", frame); 299 imshow("frame", frame);
270 if (params.stabilize && !stabilizationHomography.empty()) { 300 if (params.stabilize && !stabilizationHomography.empty()) {
271 Mat warped, vis; 301 Mat warped, vis;
272 warpPerspective(frame, warped, stabilizationHomography, videoSize); 302 warpPerspective(frame, warped, stabilizationHomography, videoSize);
273 addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis); 303 addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis);
274 imshow("warped frame", vis); 304 imshow("warped frame", vis);
275 } 305 }
276 key = waitKey(2); 306 key = waitKey(0);
277 } 307 }
278 previousFrameBW = currentFrameBW.clone(); 308 previousFrameBW = currentFrameBW.clone();
279 prevPts = currPts; 309 prevPts = currPts;
280 } 310 }
281 311