Mercurial Hosting > traffic-intelligence
diff c/feature-based-tracking.cpp @ 803:f7cf43b5ad3b dev
work in progress on stabilization
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 01 Jun 2016 01:55:45 -0400 |
parents | d3e8dd9f3ca4 |
children | 17e54690af8a |
line wrap: on
line diff
--- a/c/feature-based-tracking.cpp Tue May 31 17:59:35 2016 -0400 +++ b/c/feature-based-tracking.cpp Wed Jun 01 01:55:45 2016 -0400 @@ -13,6 +13,7 @@ #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/objdetect/objdetect.hpp" +#include <opencv2/calib3d.hpp> #include <boost/foreach.hpp> @@ -62,10 +63,13 @@ } void trackFeatures(const KLTFeatureTrackingParameters& params) { - Mat homography = ::loadMat(params.homographyFilename, " "); - Mat invHomography; - if (params.display && !homography.empty()) - invHomography = homography.inv(); + Mat refHomography = ::loadMat(params.homographyFilename, " "); + Mat stabilizationHomography, homography = refHomography; + if (params.stabilize && refHomography.empty()) + refHomography = Mat::eye(3, 3, CV_32FC1); + //Mat invHomography; + //if (params.display && !homography.empty()) + // invHomography = homography.inv(); float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; Size window = Size(params.windowSize, params.windowSize); @@ -116,7 +120,7 @@ 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); + initUndistortRectifyMap(intrinsicCameraMatrix, params.distortionCoefficients, Mat::eye(3, 3, CV_32FC1), newIntrinsicCameraMatrix, videoSize, CV_32FC1, map1, map2); cout << "Undistorted width=" << videoSize.width << ", height=" << videoSize.height << endl; @@ -146,17 +150,23 @@ int key = '?'; unsigned int savedFeatureId=0; - Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW; + Mat frame = Mat::zeros(1, 1, CV_8UC1), currentFrameBW, previousFrameBW, undistortedFrame, homographyFrameBW, homographyFrame; - Mat homographyFrame = imread("/media/disk1/Research/Data/unb/Drone/S23-10-frame.png"); // reference frame for the homography, if option turned on - if (params.undistort) { - remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); - homographyFrame = undistortedFrame; + if (params.stabilize) { + homographyFrame = imread(params.frameHomographyFilename); + if (homographyFrame.empty()) + cout << "Homography frame filename " << params.frameHomographyFilename << " could not be opened." << endl; + else { + if (params.undistort) { + remap(homographyFrame, undistortedFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); + homographyFrame = undistortedFrame; + } + + cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY); + goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? + } } - cvtColor(homographyFrame, homographyFrameBW, CV_RGB2GRAY); - goodFeaturesToTrack(homographyFrameBW, homographyRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? - unsigned int lastFrameNum = nFrames; if (params.nFrames > 0) lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); @@ -186,9 +196,22 @@ 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 there is stabilization todo - 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); + if (params.stabilize) { + 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); + std::vector<Point2f> homographyRefPts2, homographyCurrPts2; + for (unsigned int i=0; i<homographyRefPts.size(); i++) + if (homographPtsStatus[i]) { + homographyRefPts2.push_back(homographyRefPts[i]); + homographyCurrPts2.push_back(homographyCurrPts[i]); + } + stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold); + if (stabilizationHomography.empty()) + homography = refHomography; + else + homography = stabilizationHomography*refHomography; + } + std::vector<Point2f> trackedPts; std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); while (iter != featurePointMatches.end()) { @@ -196,7 +219,6 @@ if (status[iter->pointNum] && (mask.at<uchar>(static_cast<int>(round(currPts[iter->pointNum].y)), static_cast<int>(round(currPts[iter->pointNum].x))) != 0)) { iter->feature->addPoint(frameNum, currPts[iter->pointNum], homography); - deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); if (deleteFeature) @@ -223,10 +245,9 @@ saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); if (params.display) { + cout << featurePointMatches.size() << endl; BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) - fp.feature->draw(frame, invHomography, Colors::red()); - Mat homoFeatures = frame.clone(); - // todo merge the ref frame and the current frame as in the Python example + fp.feature->draw(frame, homography.inv(), Colors::red()); } } @@ -246,7 +267,12 @@ if (params.display) { imshow("mask", featureMask*256); imshow("frame", frame); - //imshow("ref homography", ) + if (params.stabilize && !stabilizationHomography.empty()) { + Mat warped, vis; + warpPerspective(frame, warped, stabilizationHomography, videoSize); + addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis); + imshow("warped frame", vis); + } key = waitKey(2); } previousFrameBW = currentFrameBW.clone();