Mercurial Hosting > traffic-intelligence
diff c/feature-based-tracking.cpp @ 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 |
line wrap: on
line diff
--- 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;