Mercurial > hg > nsaunier > traffic-intelligence

comparison c/feature-based-tracking.cpp @ 926:dbd81710d515

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
new feature tracking in image space with point undistortion
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Mon, 10 Jul 2017 18:04:41 -0400
parents a71455bd8367
children 8ac7f61c6e4f
comparison
equal deleted inserted replaced
925:974077e23804 926:dbd81710d515
112 112
113 Mat map1, map2; 113 Mat map1, map2;
114 Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix; 114 Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix;
115 if (params.undistort) { 115 if (params.undistort) {
116 intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); 116 intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " ");
117 //videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
118 // newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone();
119 // newIntrinsicCameraMatrix.at<float>(0,2) = undistortedVideoSize.width/2.;
120 // newIntrinsicCameraMatrix.at<float>(1,2) = undistortedVideoSize.height/2.;
121 Size undistortedVideoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); 117 Size undistortedVideoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication)));
122 newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);//getOptimalNewCameraMatrix(intrinsicCameraMatrix, params.distortionCoefficients, videoSize, 1, undistortedVideoSize);//, 0, true); 118 newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);//getOptimalNewCameraMatrix(intrinsicCameraMatrix, params.distortionCoefficients, videoSize, 1, undistortedVideoSize);//, 0, true);
123 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1) /* 0 ?*/, newIntrinsicCameraMatrix, undistortedVideoSize, CV_32FC1, map1, map2); 119 initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1) /* 0 ?*/, newIntrinsicCameraMatrix, undistortedVideoSize, CV_32FC1, map1, map2);
124 120
125 cout << "Undistorted width=" << undistortedVideoSize.width << 121 cout << "Undistorted width=" << undistortedVideoSize.width <<
131 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; 127 cout << "Mask filename " << params.maskFilename << " could not be opened." << endl;
132 mask = Mat::ones(videoSize, CV_8UC1); 128 mask = Mat::ones(videoSize, CV_8UC1);
133 } 129 }
134 130
135 std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>()); 131 std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>());
136 //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>();
137 trajectoryDB->connect(params.databaseFilename.c_str()); 132 trajectoryDB->connect(params.databaseFilename.c_str());
138 trajectoryDB->createTable("positions"); 133 trajectoryDB->createTable("positions");
139 trajectoryDB->createTable("velocities"); 134 trajectoryDB->createTable("velocities");
140 trajectoryDB->beginTransaction(); 135 trajectoryDB->beginTransaction();
141 136
142 std::vector<KeyPoint> prevKpts, currKpts; 137 std::vector<Point2f> prevPts, currPts, newPts, undistortedPts; // all points but undistortedPts are in image space
143 std::vector<Point2f> prevPts, currPts, newPts, undistortedPts;
144 std::vector<uchar> status; 138 std::vector<uchar> status;
145 std::vector<float> errors; 139 std::vector<float> errors;
146 Mat prevDesc, currDesc; 140 Mat prevDesc, currDesc;
147 141
148 std::vector<FeatureTrajectoryPtr> lostFeatures; 142 std::vector<FeatureTrajectoryPtr> lostFeatures;
162 if (frame.empty()) { 156 if (frame.empty()) {
163 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl; 157 cout << "Empty frame " << frameNum << ", breaking (" << frame.empty() << " [" << frame.size().width << "x" << frame.size().height << "])" << endl;
164 break; 158 break;
165 } else if (frameNum%50 ==0) 159 } else if (frameNum%50 ==0)
166 cout << "frame " << frameNum << endl; 160 cout << "frame " << frameNum << endl;
167
168 // if (params.undistort) {
169 // remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.);
170 // frame = undistortedFrame;
171
172 // if (frame.size() != videoSize) {
173 // cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl;
174 // break;
175 // }
176 // }
177 161
178 cvtColor(frame, currentFrameBW, CV_RGB2GRAY); 162 cvtColor(frame, currentFrameBW, CV_RGB2GRAY);
179 163
180 if (!prevPts.empty()) { 164 if (!prevPts.empty()) {
181 currPts.clear(); 165 currPts.clear();
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); 166 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);
183 /// \todo try calcOpticalFlowFarneback 167 /// \todo try calcOpticalFlowFarneback
184 168
185 if (params.undistort) { 169 if (params.undistort)
186 undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); 170 undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
187 //currPts = undistortedPts; 171 else
188 } 172 undistortedPts =currPts;
189 173
190 std::vector<Point2f> trackedPts; 174 std::vector<Point2f> trackedPts;
191 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); 175 std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin();
192 while (iter != featurePointMatches.end()) { 176 while (iter != featurePointMatches.end()) {
193 bool deleteFeature = false; 177 bool deleteFeature = false;
195 int currPtX = static_cast<int>(floor(currPts[iter->pointNum].x)); 179 int currPtX = static_cast<int>(floor(currPts[iter->pointNum].x));
196 int currPtY = static_cast<int>(floor(currPts[iter->pointNum].y)); 180 int currPtY = static_cast<int>(floor(currPts[iter->pointNum].y));
197 if ((status[iter->pointNum] =! 0) && 181 if ((status[iter->pointNum] =! 0) &&
198 (currPtX >= 0) && (currPtX < videoSize.width) && 182 (currPtX >= 0) && (currPtX < videoSize.width) &&
199 (currPtY >= 0) && (currPtY < videoSize.height) && 183 (currPtY >= 0) && (currPtY < videoSize.height) &&
200 (mask.at<uchar>(currPtY, currPtX) != 0)) { // todo check point in mask in image space 184 (mask.at<uchar>(currPtY, currPtX) != 0)) {
201 if (params.undistort) 185 iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography);
202 iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography);
203 else
204 iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography);
205 186
206 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) 187 deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement)
207 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); 188 || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound);
208 if (deleteFeature) 189 if (deleteFeature)
209 iter->feature->shorten(); 190 iter->feature->shorten();
244 for (unsigned int n=0;n<currPts.size(); n++) 225 for (unsigned int n=0;n<currPts.size(); n++)
245 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 226 for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++)
246 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) 227 for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++)
247 featureMask.at<uchar>(i,j)=0; 228 featureMask.at<uchar>(i,j)=0;
248 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k); 229 goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k);
249 if (params.undistort) { 230 if (params.undistort)
250 undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); 231 undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix);
251 //newPts = undistortedPts; 232 else
252 } 233 undistortedPts = newPts;
253 //BOOST_FOREACH(Point2f p, newPts) { 234
254 for (unsigned int i=0; i<newPts.size(); i++) { 235 for (unsigned int i=0; i<newPts.size(); i++) {
255 FeatureTrajectoryPtr f; 236 FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography));
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));
260 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); 237 featurePointMatches.push_back(FeaturePointMatch(f, currPts.size()));
261 currPts.push_back(newPts[i]); 238 currPts.push_back(newPts[i]);
262 } 239 }
263 240
264 if (params.display && !displayFrame.empty()) { 241 if (params.display && !displayFrame.empty()) {
265 imshow("mask", featureMask*256); 242 imshow("mask", featureMask*256);
266 imshow("frame", displayFrame); 243 imshow("frame", displayFrame);
267 key = waitKey(2); 244 key = waitKey(2);
268 } 245 }