Mercurial Hosting > traffic-intelligence
comparison c/feature-based-tracking.cpp @ 803:f7cf43b5ad3b dev
work in progress on stabilization
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 01 Jun 2016 01:55:45 -0400 |
parents | d3e8dd9f3ca4 |
children | 17e54690af8a |
comparison
equal
deleted
inserted
replaced
802:d3e8dd9f3ca4 | 803:f7cf43b5ad3b |
---|---|
11 #include "opencv2/imgproc/imgproc.hpp" | 11 #include "opencv2/imgproc/imgproc.hpp" |
12 #include "opencv2/video/tracking.hpp" | 12 #include "opencv2/video/tracking.hpp" |
13 #include "opencv2/features2d/features2d.hpp" | 13 #include "opencv2/features2d/features2d.hpp" |
14 #include "opencv2/highgui/highgui.hpp" | 14 #include "opencv2/highgui/highgui.hpp" |
15 #include "opencv2/objdetect/objdetect.hpp" | 15 #include "opencv2/objdetect/objdetect.hpp" |
16 #include <opencv2/calib3d.hpp> | |
16 | 17 |
17 #include <boost/foreach.hpp> | 18 #include <boost/foreach.hpp> |
18 | 19 |
19 #include <iostream> | 20 #include <iostream> |
20 #include <vector> | 21 #include <vector> |
60 BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName); | 61 BOOST_FOREACH(FeatureTrajectoryPtr f, features) f->write(db, positionsTableName, velocitiesTableName); |
61 features.clear(); | 62 features.clear(); |
62 } | 63 } |
63 | 64 |
64 void trackFeatures(const KLTFeatureTrackingParameters& params) { | 65 void trackFeatures(const KLTFeatureTrackingParameters& params) { |
65 Mat homography = ::loadMat(params.homographyFilename, " "); | 66 Mat refHomography = ::loadMat(params.homographyFilename, " "); |
66 Mat invHomography; | 67 Mat stabilizationHomography, homography = refHomography; |
67 if (params.display && !homography.empty()) | 68 if (params.stabilize && refHomography.empty()) |
68 invHomography = homography.inv(); | 69 refHomography = Mat::eye(3, 3, CV_32FC1); |
70 //Mat invHomography; | |
71 //if (params.display && !homography.empty()) | |
72 // invHomography = homography.inv(); | |
69 | 73 |
70 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; | 74 float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; |
71 Size window = Size(params.windowSize, params.windowSize); | 75 Size window = Size(params.windowSize, params.windowSize); |
72 | 76 |
73 int interpolationMethod = -1; | 77 int interpolationMethod = -1; |
114 Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); | 118 Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); |
115 Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); | 119 Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); |
116 videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); | 120 videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); |
117 newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.; | 121 newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.; |
118 newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.; | 122 newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.; |
119 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2); | 123 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3, 3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2); |
120 | 124 |
121 cout << "Undistorted width=" << videoSize.width << | 125 cout << "Undistorted width=" << videoSize.width << |
122 ", height=" << videoSize.height << endl; | 126 ", height=" << videoSize.height << endl; |
123 } | 127 } |
124 | 128 |
144 std::vector<FeatureTrajectoryPtr> lostFeatures; | 148 std::vector<FeatureTrajectoryPtr> lostFeatures; |
145 std::vector<FeaturePointMatch> featurePointMatches; | 149 std::vector<FeaturePointMatch> featurePointMatches; |
146 | 150 |
147 int key = '?'; | 151 int key = '?'; |
148 unsigned int savedFeatureId=0; | 152 unsigned int savedFeatureId=0; |
149 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW; | 153 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW, homographyFrame; |
150 | 154 |
151 Mat homographyFrame = imread("/media/disk1/Research/Data/unb/Drone/S23-10-frame.png"); // reference frame for the homography, if option turned on | 155 if (params.stabilize) { |
152 if (params.undistort) { | 156 homographyFrame = imread(params.frameHomographyFilename); |
153 remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); | 157 if (homographyFrame.empty()) |
154 homographyFrame = undistortedFrame; | 158 cout << "Homography frame filename " << params.frameHomographyFilename << " could not be opened." << endl; |
155 } | 159 else { |
156 | 160 if (params.undistort) { |
157 cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY); | 161 remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); |
158 goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? | 162 homographyFrame = undistortedFrame; |
163 } | |
164 | |
165 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 } | |
168 } | |
159 | 169 |
160 unsigned int lastFrameNum = nFrames; | 170 unsigned int lastFrameNum = nFrames; |
161 if (params.nFrames > 0) | 171 if (params.nFrames > 0) |
162 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); | 172 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); |
163 | 173 |
184 if (!prevPts.empty()) { | 194 if (!prevPts.empty()) { |
185 currPts.clear(); | 195 currPts.clear(); |
186 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); | 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); |
187 /// \todo try calcOpticalFlowFarneback | 197 /// \todo try calcOpticalFlowFarneback |
188 | 198 |
189 // if there is stabilization todo | 199 if (params.stabilize) { |
190 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); | 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); |
191 | 201 |
202 std::vector<Point2f> homographyRefPts2, homographyCurrPts2; | |
203 for (unsigned int i=0; i<homographyRefPts.size(); i++) | |
204 if (homographPtsStatus[i]) { | |
205 homographyRefPts2.push_back(homographyRefPts[i]); | |
206 homographyCurrPts2.push_back(homographyCurrPts[i]); | |
207 } | |
208 stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold); | |
209 if (stabilizationHomography.empty()) | |
210 homography = refHomography; | |
211 else | |
212 homography = stabilizationHomography*refHomography; | |
213 } | |
214 | |
192 std::vector<Point2f> trackedPts; | 215 std::vector<Point2f> trackedPts; |
193 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); | 216 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); |
194 while (iter != featurePointMatches.end()) { | 217 while (iter != featurePointMatches.end()) { |
195 bool deleteFeature = false; | 218 bool deleteFeature = false; |
196 | 219 |
197 if (status[iter->pointNum] && (mask.at<uchar>(static_cast<int>(round(currPts[iter->pointNum].y)), static_cast<int>(round(currPts[iter->pointNum].x))) != 0)) { | 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)) { |
198 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); | 221 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); |
199 | |
200 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) | 222 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) |
201 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); | 223 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); |
202 if (deleteFeature) | 224 if (deleteFeature) |
203 iter->feature->shorten(); | 225 iter->feature->shorten(); |
204 } else | 226 } else |
221 currPts = trackedPts; | 243 currPts = trackedPts; |
222 assert(currPts.size() == featurePointMatches.size()); | 244 assert(currPts.size() == featurePointMatches.size()); |
223 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); | 245 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); |
224 | 246 |
225 if (params.display) { | 247 if (params.display) { |
248 cout << featurePointMatches.size() << endl; | |
226 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) | 249 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) |
227 fp.feature->draw(frame, invHomography, Colors::red()); | 250 fp.feature->draw(frame, homography.inv(), Colors::red()); |
228 Mat homoFeatures = frame.clone(); | |
229 // todo merge the ref frame and the current frame as in the Python example | |
230 } | 251 } |
231 } | 252 } |
232 | 253 |
233 // adding new features, using mask around existing feature positions | 254 // adding new features, using mask around existing feature positions |
234 Mat featureMask = mask.clone(); | 255 Mat featureMask = mask.clone(); |
244 } | 265 } |
245 | 266 |
246 if (params.display) { | 267 if (params.display) { |
247 imshow("mask", featureMask*256); | 268 imshow("mask", featureMask*256); |
248 imshow("frame", frame); | 269 imshow("frame", frame); |
249 //imshow("ref homography", ) | 270 if (params.stabilize && !stabilizationHomography.empty()) { |
271 Mat warped, vis; | |
272 warpPerspective(frame, warped, stabilizationHomography, videoSize); | |
273 addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis); | |
274 imshow("warped frame", vis); | |
275 } | |
250 key = waitKey(2); | 276 key = waitKey(2); |
251 } | 277 } |
252 previousFrameBW = currentFrameBW.clone(); | 278 previousFrameBW = currentFrameBW.clone(); |
253 prevPts = currPts; | 279 prevPts = currPts; |
254 } | 280 } |