Mercurial Hosting > traffic-intelligence
comparison c/feature-based-tracking.cpp @ 804:17e54690af8a dev
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 |