Mercurial Hosting > traffic-intelligence
diff 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 |
line wrap: on
line diff
--- a/c/feature-based-tracking.cpp Mon Jul 10 01:38:12 2017 -0400 +++ b/c/feature-based-tracking.cpp Mon Jul 10 18:04:41 2017 -0400 @@ -114,10 +114,6 @@ Mat intrinsicCameraMatrix, newIntrinsicCameraMatrix; if (params.undistort) { 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); @@ -133,14 +129,12 @@ } std::shared_ptr<TrajectoryDBAccess<Point2f> > trajectoryDB = std::shared_ptr<TrajectoryDBAccess<Point2f> >(new TrajectoryDBAccessList<Point2f>()); - //TrajectoryDBAccess<Point2f>* trajectoryDB = new TrajectoryDBAccessBlob<Point2f>(); trajectoryDB->connect(params.databaseFilename.c_str()); trajectoryDB->createTable("positions"); trajectoryDB->createTable("velocities"); trajectoryDB->beginTransaction(); - std::vector<KeyPoint> prevKpts, currKpts; - std::vector<Point2f> prevPts, currPts, newPts, undistortedPts; + std::vector<Point2f> prevPts, currPts, newPts, undistortedPts; // all points but undistortedPts are in image space std::vector<uchar> status; std::vector<float> errors; Mat prevDesc, currDesc; @@ -164,16 +158,6 @@ break; } else if (frameNum%50 ==0) cout << "frame " << frameNum << endl; - - // 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; - // } - // } cvtColor(frame, currentFrameBW, CV_RGB2GRAY); @@ -182,10 +166,10 @@ 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) { + if (params.undistort) undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); - //currPts = undistortedPts; - } + else + undistortedPts =currPts; std::vector<Point2f> trackedPts; std::vector<FeaturePointMatch>::iterator iter = featurePointMatches.begin(); @@ -197,11 +181,8 @@ if ((status[iter->pointNum] =! 0) && (currPtX >= 0) && (currPtX < videoSize.width) && (currPtY >= 0) && (currPtY < videoSize.height) && - (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); + (mask.at<uchar>(currPtY, currPtX) != 0)) { + iter->feature->addPoint(frameNum, undistortedPts[iter->pointNum], homography); deleteFeature = iter->feature->isDisplacementSmall(params.nDisplacements, minTotalFeatureDisplacement) || !iter->feature->isMotionSmooth(params.accelerationBound, params.deviationBound); @@ -246,21 +227,17 @@ 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); - if (params.undistort) { + if (params.undistort) undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); - //newPts = undistortedPts; - } - //BOOST_FOREACH(Point2f p, newPts) { + else + undistortedPts = 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)); + FeatureTrajectoryPtr f = FeatureTrajectoryPtr(new FeatureTrajectory(frameNum, undistortedPts[i], homography)); featurePointMatches.push_back(FeaturePointMatch(f, currPts.size())); currPts.push_back(newPts[i]); } - + if (params.display && !displayFrame.empty()) { imshow("mask", featureMask*256); imshow("frame", displayFrame);