Mercurial Hosting > traffic-intelligence
changeset 804:17e54690af8a dev
work in progress, not fully functional yet
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Wed, 01 Jun 2016 17:57:49 -0400 |
parents | f7cf43b5ad3b |
children | dceaca7e1c97 |
files | c/Motion.cpp c/cvutils.cpp c/feature-based-tracking.cpp include/Motion.hpp include/cvutils.hpp |
diffstat | 5 files changed, 106 insertions(+), 70 deletions(-) [+] |
line wrap: on
line diff
--- a/c/Motion.cpp Wed Jun 01 01:55:45 2016 -0400 +++ b/c/Motion.cpp Wed Jun 01 17:57:49 2016 -0400 @@ -22,11 +22,11 @@ /******************** FeatureTrajectory ********************/ -FeatureTrajectory::FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p, const Mat& homography) +FeatureTrajectory::FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p) : firstInstant(frameNum), lastInstant(frameNum) { positions = TrajectoryPoint2fPtr(new TrajectoryPoint2f()); velocities = TrajectoryPoint2fPtr(new TrajectoryPoint2f()); - addPoint(frameNum, p, homography); + addPoint(frameNum, p); } FeatureTrajectory::FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities) { @@ -93,11 +93,8 @@ return connected; } -void FeatureTrajectory::addPoint(const unsigned int& frameNum, const Point2f& p, const Mat& homography) { - Point2f pp = p; - if (!homography.empty()) - pp = project(p, homography); - positions->add(frameNum, pp); +void FeatureTrajectory::addPoint(const unsigned int& frameNum, const Point2f& p) { + positions->add(frameNum, p); if (frameNum < firstInstant) firstInstant = frameNum; if (frameNum > lastInstant) @@ -128,12 +125,12 @@ void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const { Point2f p1, p2; if (!homography.empty()) - p1 = project((*positions)[0], homography); + p1 = project<double>((*positions)[0], homography); else p1 = (*positions)[0]; for (unsigned int i=1; i<positions->size(); i++) { if (!homography.empty()) - p2 = project((*positions)[i], homography); + p2 = project<double>((*positions)[i], homography); else p2 = (*positions)[i]; line(img, p1, p2, color, 1);
--- a/c/cvutils.cpp Wed Jun 01 01:55:45 2016 -0400 +++ b/c/cvutils.cpp Wed Jun 01 17:57:49 2016 -0400 @@ -11,19 +11,18 @@ using namespace std; using namespace cv; -Point2f project(const Point2f& p, const Mat& homography) { - //Mat homogeneous(3, 1, CV_32FC1); - float x, y; - float w = homography.at<float>(2,0)*p.x+homography.at<float>(2,1)*p.y+homography.at<float>(2,2); - if (w != 0) { - x = (homography.at<float>(0,0)*p.x+homography.at<float>(0,1)*p.y+homography.at<float>(0,2))/w; - y = (homography.at<float>(1,0)*p.x+homography.at<float>(1,1)*p.y+homography.at<float>(1,2))/w; - } else { - x = 0; - y = 0; - } - return Point2f(x, y); -} +// Point2f project(const Point2f& p, const Mat& homography) { +// float x, y; +// float w = homography.at<float>(2,0)*p.x+homography.at<float>(2,1)*p.y+homography.at<float>(2,2); +// if (w != 0) { +// x = (homography.at<float>(0,0)*p.x+homography.at<float>(0,1)*p.y+homography.at<float>(0,2))/w; +// y = (homography.at<float>(1,0)*p.x+homography.at<float>(1,1)*p.y+homography.at<float>(1,2))/w; +// } else { +// x = 0; +// y = 0; +// } +// return Point2f(x, y); +// } Mat loadMat(const string& filename, const string& separator) { vector<vector<float> > numbers = ::loadNumbers(filename, separator);
--- a/c/feature-based-tracking.cpp Wed Jun 01 01:55:45 2016 -0400 +++ b/c/feature-based-tracking.cpp Wed Jun 01 17:57:49 2016 -0400 @@ -64,13 +64,13 @@ void trackFeatures(const KLTFeatureTrackingParameters& params) { 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(); - + //if (params.stabilize && refHomography.empty()) + // refHomography = Mat::eye(3, 3, CV_64FC1); + //Mat stabilizationHomography(3,3,CV_64FC1), homography(3,3,CV_64FC1); + Mat stabilizationHomography, homography; + if (!params.stabilize) + homography = refHomography; + float minTotalFeatureDisplacement = params.nDisplacements*params.minFeatureDisplacement; Size window = Size(params.windowSize, params.windowSize); @@ -131,6 +131,7 @@ cout << "Mask filename " << params.maskFilename << " could not be opened." << endl; mask = Mat::ones(videoSize, CV_8UC1); } + Mat featureMask = mask.clone(); std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>()); //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>(); @@ -140,9 +141,9 @@ trajectoryDB->beginTransaction(); std::vector<KeyPoint> prevKpts, currKpts; - std::vector<Point2f> prevPts, currPts, newPts, homographyRefPts, homographyCurrPts; - std::vector<uchar> status, homographPtsStatus; - std::vector<float> errors, homographyErrors; + std::vector<Point2f> prevPts, currPts, newPts, stabilizationRefPts, stabilizationCurrPts; + std::vector<uchar> status, stabilizationPtsStatus; + std::vector<float> errorPts, stabilizationErrorPts; Mat prevDesc, currDesc; std::vector<FeatureTrajectoryPtr> lostFeatures; @@ -163,7 +164,7 @@ } 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? + goodFeaturesToTrack(homographyFrameBW, stabilizationRefPts, params.maxNFeatures, MAX(0.01, params.featureQuality), params.minFeatureDistanceKLT, mask, params.blockSize, params.useHarrisDetector, params.k); // take higher quality features? } } @@ -171,6 +172,7 @@ if (params.nFrames > 0) lastFrameNum = MIN(params.frame1+static_cast<unsigned int>(params.nFrames), nFrames); + // Main Loop capture.set(CV_CAP_PROP_POS_FRAMES, params.frame1); for (unsigned int frameNum = params.frame1; (frameNum < lastFrameNum) && !::interruptionKey(key); frameNum++) { capture >> frame; @@ -193,36 +195,57 @@ if (!prevPts.empty()) { currPts.clear(); - 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); + calcOpticalFlowPyrLK(previousFrameBW, currentFrameBW, prevPts, currPts, status, errorPts, 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.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); + calcOpticalFlowPyrLK(homographyFrameBW, currentFrameBW, stabilizationRefPts, stabilizationCurrPts, stabilizationPtsStatus, stabilizationErrorPts, 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]); + std::vector<Point2f> stabilizationRefPts2, stabilizationCurrPts2; + for (unsigned int i=0; i<stabilizationRefPts.size(); i++) + if (stabilizationPtsStatus[i]) { + stabilizationRefPts2.push_back(stabilizationRefPts[i]); + stabilizationCurrPts2.push_back(stabilizationCurrPts[i]); } - stabilizationHomography = findHomography(homographyCurrPts2, homographyRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold); - if (stabilizationHomography.empty()) + stabilizationHomography = findHomography(stabilizationCurrPts2, stabilizationRefPts2, CV_RANSAC, params.stabilizeRansacReprojThreshold); + //cout << stabilizationHomography << endl; + //cout << stabilizationHomography.type() << " " << refHomography.type() << endl; + //for (unsigned int i=0; i<MIN(stabilizationRefPts.size(), 10); i++) { + //cout << stabilizationCurrPts2[i] << " " << stabilizationRefPts2[i] << ": projected " << ::project<double>(stabilizationCurrPts2[i], stabilizationHomography) << endl; + //cout << stabilizationHomography.at<double>(0,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(0,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(0,2) << " " << stabilizationHomography.at<double>(1,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(1,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(1,2) << " " << stabilizationHomography.at<double>(2,0)*stabilizationCurrPts2[i].x+stabilizationHomography.at<double>(2,1)*stabilizationCurrPts2[i].y+stabilizationHomography.at<double>(2,2) << endl; + //} + if (stabilizationHomography.empty()) { homography = refHomography; - else - homography = stabilizationHomography*refHomography; - } + featureMask = mask.clone(); + } else { + if (refHomography.empty()) + homography = stabilizationHomography; + else + homography = refHomography*stabilizationHomography; + //cout << refHomography << " * " << stabilizationHomography << " = " << homography << endl; + warpPerspective(mask, featureMask, stabilizationHomography, videoSize, INTER_LINEAR+WARP_INVERSE_MAP, BORDER_CONSTANT, 0); + } + } else + featureMask = mask.clone(); + std::vector<Point2f> trackedPts; std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); while (iter != featurePointMatches.end()) { bool deleteFeature = false; - - 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) - iter->feature->shorten(); + + if (status[iter->pointNum]) { + Point2f homographyFramePt = ::project<double>(currPts[iter->pointNum], stabilizationHomography); // project to space of homography frame (if not stabilization, stabilizationHomography should be empty) + cout << currPts[iter->pointNum] << ", " << homographyFramePt << endl; + if (::inImage(homographyFramePt, videoSize) && (mask.at<uchar>(static_cast<int>(round(homographyFramePt.y)), static_cast<int>(round(homographyFramePt.x))) != 0)) { // check point is in mask + iter->feature->addPoint(frameNum, ::project<double>(homographyFramePt, refHomography)); + //cout << *(iter->feature) << endl; + deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) + || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); + if (deleteFeature) + iter->feature->shorten(); + } else + deleteFeature = true; } else deleteFeature = true; @@ -245,27 +268,34 @@ saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); if (params.display) { - cout << featurePointMatches.size() << endl; + cout << featurePointMatches.size() << " matches" << endl; + Mat invHomography; + if (!homography.empty()) + invHomography = homography.inv(); BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) - fp.feature->draw(frame, homography.inv(), Colors::red()); + fp.feature->draw(frame, invHomography, Colors::red()); } } // adding new features, using mask around existing feature positions - Mat featureMask = mask.clone(); for (unsigned int n=0;n<currPts.size(); n++) for (int j=MAX(0, currPts[n].x-params.minFeatureDistanceKLT); j<MIN(videoSize.width, currPts[n].x+params.minFeatureDistanceKLT+1); j++) 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)); + FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, ::project<double>(::project<double>(p, stabilizationHomography), refHomography))); featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); currPts.push_back(p); } if (params.display) { - imshow("mask", featureMask*256); + cout << currPts.size() << " current points" << endl; + Mat maskedFrame; + cvtColor(featureMask*256, maskedFrame, CV_GRAY2RGB); + addWeighted(frame, 0.5, maskedFrame, 0.5, 0.0, maskedFrame); + imshow("masked frame", maskedFrame); + //imshow("mask", featureMask*256); imshow("frame", frame); if (params.stabilize && !stabilizationHomography.empty()) { Mat warped, vis; @@ -273,7 +303,7 @@ addWeighted(homographyFrame, 0.5, warped, 0.5, 0.0, vis); imshow("warped frame", vis); } - key = waitKey(2); + key = waitKey(0); } previousFrameBW = currentFrameBW.clone(); prevPts = currPts;
--- a/include/Motion.hpp Wed Jun 01 01:55:45 2016 -0400 +++ b/include/Motion.hpp Wed Jun 01 17:57:49 2016 -0400 @@ -15,7 +15,7 @@ class FeatureTrajectory { public: - FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p, const cv::Mat& homography); + FeatureTrajectory(const unsigned int& frameNum, const cv::Point2f& p); FeatureTrajectory(TrajectoryPoint2fPtr& _positions, TrajectoryPoint2fPtr& _velocities); @@ -43,7 +43,7 @@ /// computes the distance according to the Beymer et al. algorithm bool minMaxSimilarity(const FeatureTrajectory& ft, const int& firstInstant, const int& lastInstant, const float& connectionDistance, const float& segmentationDistance); - void addPoint(const unsigned int& frameNum, const cv::Point2f& p, const cv::Mat& homography); + void addPoint(const unsigned int& frameNum, const cv::Point2f& p); void shorten(void);
--- a/include/cvutils.hpp Wed Jun 01 01:55:45 2016 -0400 +++ b/include/cvutils.hpp Wed Jun 01 17:57:49 2016 -0400 @@ -5,28 +5,38 @@ #include "opencv2/features2d/features2d.hpp" class CvCapture; -//template<typename T> class Point_<T>; - -/// constant that indicates if the image should be flipped -//static const int flipImage = CV_CVTIMG_FLIP; /** Projects a point with the homography matrix use perspectiveTransform for arrays of points. */ -cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography); +//cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography); +template<typename T> +inline cv::Point_<T> project(const cv::Point_<T>& p, const cv::Mat_<T>& homography) { + if (homography.empty()) + return p; + else { + T x, y; + T w = homography.template at<T>(2,0)*p.x+homography.template at<T>(2,1)*p.y+homography.template at<T>(2,2); + if (w != 0.) { + x = (homography.template at<T>(0,0)*p.x+homography.template at<T>(0,1)*p.y+homography.template at<T>(0,2))/w; + y = (homography.template at<T>(1,0)*p.x+homography.template at<T>(1,1)*p.y+homography.template at<T>(1,2))/w; + } else { + x = 0.; + y = 0.; + } + return cv::Point_<T>(x, y); + } +} /** Loads a cv mat from a text file where the numbers are saved line by line separated by separator */ cv::Mat loadMat(const std::string& filename, const std::string& separator); +inline bool inImage(const cv::Point2f& p, const cv::Size s) { return (p.x >= 0) && (p.x <= s.width) && (p.y >= 0) && (p.y <= s.height); } + //template<typename T> //float scalarProduct(const cv::Point_<T>& v1, const cv::Point_<T>& v2) { return v1.x*v2.x+v1.y*v2.y;} void keyPoints2Points(const std::vector<cv::KeyPoint>& kpts, std::vector<cv::Point2f>& pts, const bool& clearPts = true); -/** Allocates a new IplImage. */ -// IplImage* allocateImage(const int& width, const int& height, const int& depth, const int& channels); - -// IplImage* allocateImage(const CvSize& size, const int& depth, const int& channels); - /** Goes to the target frame number, by querying frame, supposing the video input is currently at current frame number. Returns the frame number that was reached.*/