Mercurial Hosting > traffic-intelligence
diff c/feature-based-tracking.cpp @ 924:a71455bd8367
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 |
line wrap: on
line diff
--- a/c/feature-based-tracking.cpp Wed Jul 05 23:01:24 2017 -0400 +++ b/c/feature-based-tracking.cpp Fri Jul 07 18:01:45 2017 -0400 @@ -13,6 +13,7 @@ #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/objdetect/objdetect.hpp" +#include "opencv2/calib3d/calib3d.hpp" #include <boost/foreach.hpp> @@ -110,16 +111,19 @@ ", nframes=" << nFrames << endl; Mat map1, map2; + Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix; if (params.undistort) { - Mat intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); - Mat newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); - videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); - newIntrinsicCameraMatrix.at<float>(0,2) = videoSize.width/2.; - newIntrinsicCameraMatrix.at<float>(1,2) = videoSize.height/2.; - initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2); + intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); + //videoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); + // newIntrinsicCameraMatrix = intrinsicCameraMatrix.clone(); + // newIntrinsicCameraMatrix.at<float>(0,2) = undistortedVideoSize.width/2.; + // newIntrinsicCameraMatrix.at<float>(1,2) = undistortedVideoSize.height/2.; + Size undistortedVideoSize = Size(static_cast<int>(round(videoSize.width*params.undistortedImageMultiplication)), static_cast<int>(round(videoSize.height*params.undistortedImageMultiplication))); + newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);//getOptimalNewCameraMatrix(intrinsicCameraMatrix, params.distortionCoefficients, videoSize, 1, undistortedVideoSize);//, 0, true); + initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3,3, CV_32FC1) /* 0 ?*/, newIntrinsicCameraMatrix, undistortedVideoSize, CV_32FC1, map1, map2); - cout << "Undistorted width=" << videoSize.width << - ", height=" << videoSize.height << endl; + cout << "Undistorted width=" << undistortedVideoSize.width << + ", height=" << undistortedVideoSize.height << endl; } Mat mask = imread(params.maskFilename, 0); @@ -136,7 +140,7 @@ trajectoryDB->beginTransaction(); std::vector<KeyPoint> prevKpts, currKpts; - std::vector<Point2f> prevPts, currPts, newPts; + std::vector<Point2f> prevPts, currPts, newPts, undistortedPts; std::vector<uchar> status; std::vector<float> errors; Mat prevDesc, currDesc; @@ -146,7 +150,7 @@ int key = '?'; unsigned int savedFeatureId=0; - Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame; + Mat frame, currentFrameBW, previousFrameBW, displayFrame; // = Mat::zeros(1, 1, CV_8UC1) unsigned int lastFrameNum = nFrames; if (params.nFrames > 0) @@ -161,15 +165,15 @@ } else if (frameNum%50 ==0) cout << "frame " << frameNum << endl; - if (params.undistort) { - remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); - frame = undistortedFrame; + // if (params.undistort) { + // remap(frame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); + // frame = undistortedFrame; - if (frame.size() != videoSize) { - cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; - break; - } - } + // if (frame.size() != videoSize) { + // cout << "Different frame size " << frameNum << ", breaking ([" << frame.size().width << "x" << frame.size().height << "])" << endl; + // break; + // } + // } cvtColor(frame, currentFrameBW, CV_RGB2GRAY); @@ -178,6 +182,11 @@ 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); /// \todo try calcOpticalFlowFarneback + if (params.undistort) { + undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); + //currPts = undistortedPts; + } + std::vector<Point2f> trackedPts; std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); while (iter != featurePointMatches.end()) { @@ -185,11 +194,14 @@ int currPtX = static_cast<int>(floor(currPts[iter->pointNum].x)); int currPtY = static_cast<int>(floor(currPts[iter->pointNum].y)); - if ((status[iter->pointNum] =!0) && + if ((status[iter->pointNum] =! 0) && (currPtX >= 0) && (currPtX < videoSize.width) && (currPtY >= 0) && (currPtY < videoSize.height) && - (mask.at<uchar>(currPtY, currPtX) != 0)) { - iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); + (mask.at<uchar>(currPtY, currPtX) != 0)) { // todo check point in mask in image space + if (params.undistort) + iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography); + else + iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); @@ -217,8 +229,13 @@ saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); if (params.display) { + if (params.undistort) + remap(frame, displayFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); + else + displayFrame = frame.clone(); + BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) - fp.feature->draw(frame, invHomography, Colors::red()); + fp.feature->draw(displayFrame, invHomography, Colors::red()); } } @@ -229,15 +246,24 @@ for (int i=MAX(0, currPts[n].y-params.minFeatureDistanceKLT); i<MIN(videoSize.height, currPts[n].y+params.minFeatureDistanceKLT+1); i++) featureMask.at<uchar>(i,j)=0; goodFeaturesToTrack(currentFrameBW, newPts, params.maxNFeatures, params.featureQuality, params.minFeatureDistanceKLT, featureMask, params.blockSize, params.useHarrisDetector, params.k); - BOOST_FOREACH(Point2f p, newPts) { - FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, p, homography)); + if (params.undistort) { + undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); + //newPts = undistortedPts; + } + //BOOST_FOREACH(Point2f p, newPts) { + for (unsigned int i=0; i<newPts.size(); i++) { + FeatureTrajectoryPtr f; + if (params.undistort) // write function + f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography)); + else + f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, newPts[i], homography)); featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); - currPts.push_back(p); + currPts.push_back(newPts[i]); } - if (params.display) { + if (params.display && !displayFrame.empty()) { imshow("mask", featureMask*256); - imshow("frame", frame); + imshow("frame", displayFrame); key = waitKey(2); } previousFrameBW = currentFrameBW.clone();