Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 803:f7cf43b5ad3b dev

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 }