Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 924:a71455bd8367

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
work in progress on undistortion acceleration
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 07 Jul 2017 18:01:45 -0400
parents daa992ac6b44
children dbd81710d515
comparison
equal deleted inserted replaced
923:238008f81c16 924:a71455bd8367
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/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>
108 ": width=" << videoSize.width << 109 ": width=" << videoSize.width <<
109 ", height=" << videoSize.height << 110 ", height=" << videoSize.height <<
110 ", nframes=" << nFrames << endl; 111 ", nframes=" << nFrames << endl;
111 112
112 Mat map1, map2; 113 Mat map1, map2;
114 Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix;
113 if (params.undistort) { 115 if (params.undistort) {
114 Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); 116 intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
115 Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); 117 //videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
116 videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); 118 // newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone();
117 newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.; 119 // newIntrinsicCameraMatrix.at<float>(0,2) = undistortedVideoSize.width/2.;
118 newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.; 120 // newIntrinsicCameraMatrix.at<float>(1,2) = undistortedVideoSize.height/2.;
119 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2); 121 Size undistortedVideoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
120 122 newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);//getOptimalNewCameraMatrix(intrinsicCameraMatrix, params.distortionCoefficients, videoSize, 1, undistortedVideoSize);//, 0, true);
121 cout << "Undistorted width=" << videoSize.width << 123 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1) /* 0 ?*/, newIntrinsicCameraMatrix, undistortedVideoSize, CV_32FC1, map1, map2);
122 ", height=" << videoSize.height << endl; 124
125 cout << "Undistorted width=" << undistortedVideoSize.width <<
126 ", height=" << undistortedVideoSize.height << endl;
123 } 127 }
124 128
125 Mat mask = imread(params.maskFilename, 0); 129 Mat mask = imread(params.maskFilename, 0);
126 if (mask.empty()) { 130 if (mask.empty()) {
127 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; 131 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
134 trajectoryDB->createTable("positions"); 138 trajectoryDB->createTable("positions");
135 trajectoryDB->createTable("velocities"); 139 trajectoryDB->createTable("velocities");
136 trajectoryDB->beginTransaction(); 140 trajectoryDB->beginTransaction();
137 141
138 std::vector<KeyPoint> prevKpts, currKpts; 142 std::vector<KeyPoint> prevKpts, currKpts;
139 std::vector<Point2f> prevPts, currPts, newPts; 143 std::vector<Point2f> prevPts, currPts, newPts, undistortedPts;
140 std::vector<uchar> status; 144 std::vector<uchar> status;
141 std::vector<float> errors; 145 std::vector<float> errors;
142 Mat prevDesc, currDesc; 146 Mat prevDesc, currDesc;
143 147
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; 153 Mat frame, currentFrameBW, previousFrameBW, displayFrame; // = Mat::zeros(1, 1, CV_8UC1)
150 154
151 unsigned int lastFrameNum = nFrames; 155 unsigned int lastFrameNum = nFrames;
152 if (params.nFrames > 0) 156 if (params.nFrames > 0)
153 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); 157 lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames);
154 158
159 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; 163 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl;
160 break; 164 break;
161 } else if (frameNum%50 ==0) 165 } else if (frameNum%50 ==0)
162 cout << "frame " << frameNum << endl; 166 cout << "frame " << frameNum << endl;
163 167
164 if (params.undistort) { 168 // if (params.undistort) {
165 remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); 169 // remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
166 frame = undistortedFrame; 170 // frame = undistortedFrame;
167 171
168 if (frame.size() != videoSize) { 172 // if (frame.size() != videoSize) {
169 cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; 173 // cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl;
170 break; 174 // break;
171 } 175 // }
172 } 176 // }
173 177
174 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); 178 cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
175 179
176 if (!prevPts.empty()) { 180 if (!prevPts.empty()) {
177 currPts.clear(); 181 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); 182 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 183 /// \todo try calcOpticalFlowFarneback
180 184
185 if (params.undistort) {
186 undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
187 //currPts = undistortedPts;
188 }
189
181 std::vector<Point2f> trackedPts; 190 std::vector<Point2f> trackedPts;
182 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 191 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
183 while (iter != featurePointMatches.end()) { 192 while (iter != featurePointMatches.end()) {
184 bool deleteFeature = false; 193 bool deleteFeature = false;
185 194
186 int currPtX = static_cast<int>(floor(currPts[iter->pointNum].x)); 195 int currPtX = static_cast<int>(floor(currPts[iter->pointNum].x));
187 int currPtY = static_cast<int>(floor(currPts[iter->pointNum].y)); 196 int currPtY = static_cast<int>(floor(currPts[iter->pointNum].y));
188 if ((status[iter->pointNum] =!0) && 197 if ((status[iter->pointNum] =! 0) &&
189 (currPtX >= 0) && (currPtX < videoSize.width) && 198 (currPtX >= 0) && (currPtX < videoSize.width) &&
190 (currPtY >= 0) && (currPtY < videoSize.height) && 199 (currPtY >= 0) && (currPtY < videoSize.height) &&
191 (mask.at<uchar>(currPtY, currPtX) != 0)) { 200 (mask.at<uchar>(currPtY, currPtX) != 0)) { // todo check point in mask in image space
192 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); 201 if (params.undistort)
202 iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography);
203 else
204 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
193 205
194 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) 206 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
195 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); 207 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
196 if (deleteFeature) 208 if (deleteFeature)
197 iter->feature->shorten(); 209 iter->feature->shorten();
215 currPts = trackedPts; 227 currPts = trackedPts;
216 assert(currPts.size() == featurePointMatches.size()); 228 assert(currPts.size() == featurePointMatches.size());
217 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); 229 saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities");
218 230
219 if (params.display) { 231 if (params.display) {
232 if (params.undistort)
233 remap(frame, displayFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
234 else
235 displayFrame = frame.clone();
236
220 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) 237 BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches)
221 fp.feature->draw(frame, invHomography, Colors::red()); 238 fp.feature->draw(displayFrame, invHomography, Colors::red());
222 } 239 }
223 } 240 }
224 241
225 // adding new features, using mask around existing feature positions 242 // adding new features, using mask around existing feature positions
226 Mat featureMask = mask.clone(); 243 Mat featureMask = mask.clone();
227 for (unsigned int n=0;n<currPts.size(); n++) 244 for (unsigned int n=0;n<currPts.size(); n++)
228 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 245 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
229 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) 246 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
230 featureMask.at<uchar>(i,j)=0; 247 featureMask.at<uchar>(i,j)=0;
231 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k); 248 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
232 BOOST_FOREACH(Point2f p, newPts) { 249 if (params.undistort) {
233 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography)); 250 undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
251 //newPts = undistortedPts;
252 }
253 //BOOST_FOREACH(Point2f p, newPts) {
254 for (unsigned int i=0; i<newPts.size(); i++) {
255 FeatureTrajectoryPtr f;
256 if (params.undistort) // write function
257 f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography));
258 else
259 f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, newPts[i], homography));
234 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); 260 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
235 currPts.push_back(p); 261 currPts.push_back(newPts[i]);
236 } 262 }
237 263
238 if (params.display) { 264 if (params.display && !displayFrame.empty()) {
239 imshow("mask", featureMask*256); 265 imshow("mask", featureMask*256);
240 imshow("frame", frame); 266 imshow("frame", displayFrame);
241 key = waitKey(2); 267 key = waitKey(2);
242 } 268 }
243 previousFrameBW = currentFrameBW.clone(); 269 previousFrameBW = currentFrameBW.clone();
244 prevPts = currPts; 270 prevPts = currPts;
245 } 271 }