Mercurial Hosting > traffic-intelligence
comparison c/feature-based-tracking.cpp @ 926:dbd81710d515
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 } |