Mercurial Hosting > traffic-intelligence
comparison c/feature-based-tracking.cpp @ 802:d3e8dd9f3ca4 dev
current dev for drone stabilization
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Tue, 31 May 2016 17:59:35 -0400 |
parents | 2cade72d75ad |
children | f7cf43b5ad3b |
comparison
equal
deleted
inserted
replaced
800:2cade72d75ad | 802:d3e8dd9f3ca4 |
---|---|
134 trajectoryDB->createTable("positions"); | 134 trajectoryDB->createTable("positions"); |
135 trajectoryDB->createTable("velocities"); | 135 trajectoryDB->createTable("velocities"); |
136 trajectoryDB->beginTransaction(); | 136 trajectoryDB->beginTransaction(); |
137 | 137 |
138 std::vector<KeyPoint> prevKpts, currKpts; | 138 std::vector<KeyPoint> prevKpts, currKpts; |
139 std::vector<Point2f> prevPts, currPts, newPts; | 139 std::vector<Point2f> prevPts, currPts, newPts, homographyRefPts, homographyCurrPts; |
140 std::vector<uchar> status; | 140 std::vector<uchar> status, homographPtsStatus; |
141 std::vector<float> errors; | 141 std::vector<float> errors, homographyErrors; |
142 Mat prevDesc, currDesc; | 142 Mat prevDesc, currDesc; |
143 | 143 |
144 std::vector<FeatureTrajectoryPtr> lostFeatures; | 144 std::vector<FeatureTrajectoryPtr> lostFeatures; |
145 std::vector<FeaturePointMatch> featurePointMatches; | 145 std::vector<FeaturePointMatch> featurePointMatches; |
146 | 146 |
147 int key = '?'; | 147 int key = '?'; |
148 unsigned int savedFeatureId=0; | 148 unsigned int savedFeatureId=0; |
149 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame; | 149 Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW; |
150 | |
151 Mat homographyFrame = imread("/media/disk1/Research/Data/unb/Drone/S23-10-frame.png"); // reference frame for the homography, if option turned on | |
152 if (params.undistort) { | |
153 remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); | |
154 homographyFrame = undistortedFrame; | |
155 } | |
156 | |
157 cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY); | |
158 goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? | |
150 | 159 |
151 unsigned int lastFrameNum = nFrames; | 160 unsigned int lastFrameNum = nFrames; |
152 if (params.nFrames > 0) | 161 if (params.nFrames > 0) |
153 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); | 162 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); |
154 | 163 |
159 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; | 168 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; |
160 break; | 169 break; |
161 } else if (frameNum%50 ==0) | 170 } else if (frameNum%50 ==0) |
162 cout << "frame " << frameNum << endl; | 171 cout << "frame " << frameNum << endl; |
163 | 172 |
173 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); | |
174 | |
164 if (params.undistort) { | 175 if (params.undistort) { |
165 remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); | 176 remap(currentFrameBW, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); |
166 frame = undistortedFrame; | 177 currentFrameBW = undistortedFrame; |
167 | |
168 if (frame.size() != videoSize) { | 178 if (frame.size() != videoSize) { |
169 cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; | 179 cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; |
170 break; | 180 break; |
171 } | 181 } |
172 } | 182 } |
173 | 183 |
174 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); | |
175 | |
176 if (!prevPts.empty()) { | 184 if (!prevPts.empty()) { |
177 currPts.clear(); | 185 currPts.clear(); |
178 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); | 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); |
179 /// \todo try calcOpticalFlowFarneback | 187 /// \todo try calcOpticalFlowFarneback |
188 | |
189 // if there is stabilization todo | |
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); | |
180 | 191 |
181 std::vector<Point2f> trackedPts; | 192 std::vector<Point2f> trackedPts; |
182 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); | 193 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); |
183 while (iter != featurePointMatches.end()) { | 194 while (iter != featurePointMatches.end()) { |
184 bool deleteFeature = false; | 195 bool deleteFeature = false; |
212 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); | 223 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); |
213 | 224 |
214 if (params.display) { | 225 if (params.display) { |
215 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) | 226 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) |
216 fp.feature->draw(frame, invHomography, Colors::red()); | 227 fp.feature->draw(frame, invHomography, Colors::red()); |
228 Mat homoFeatures = frame.clone(); | |
229 // todo merge the ref frame and the current frame as in the Python example | |
217 } | 230 } |
218 } | 231 } |
219 | 232 |
220 // adding new features, using mask around existing feature positions | 233 // adding new features, using mask around existing feature positions |
221 Mat featureMask = mask.clone(); | 234 Mat featureMask = mask.clone(); |
231 } | 244 } |
232 | 245 |
233 if (params.display) { | 246 if (params.display) { |
234 imshow("mask", featureMask*256); | 247 imshow("mask", featureMask*256); |
235 imshow("frame", frame); | 248 imshow("frame", frame); |
249 //imshow("ref homography", ) | |
236 key = waitKey(2); | 250 key = waitKey(2); |
237 } | 251 } |
238 previousFrameBW = currentFrameBW.clone(); | 252 previousFrameBW = currentFrameBW.clone(); |
239 prevPts = currPts; | 253 prevPts = currPts; |
240 } | 254 } |