Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 802:d3e8dd9f3ca4 dev

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