Mercurial Hosting > traffic-intelligence
changeset 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 | 974077e23804 |
children | c030f735c594 |
files | c/feature-based-tracking.cpp python/cvutils.py scripts/undistort-video.py |
diffstat | 3 files changed, 28 insertions(+), 43 deletions(-) [+] |
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);
--- a/python/cvutils.py Mon Jul 10 01:38:12 2017 -0400 +++ b/python/cvutils.py Mon Jul 10 18:04:41 2017 -0400 @@ -122,9 +122,6 @@ def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients): newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication))) - #newCameraMatrix = deepcopy(intrinsicCameraMatrix) - #newCameraMatrix[0,2] = newImgSize[0]/2. - #newCameraMatrix[1,2] = newImgSize[1]/2. newCameraMatrix = cv2.getDefaultNewCameraMatrix(intrinsicCameraMatrix, newImgSize, True) return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), None, newCameraMatrix, newImgSize, cv2.CV_32FC1) @@ -432,7 +429,7 @@ invMap2[y,x] = res[1] return invMap1, invMap2 - def cameraIntrinsicCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False): + def intrinsicCameraCalibration(path, checkerBoardSize=[6,7], secondPassSearch=False, display=False, fixK2 = True, fixK3 = True, zeroTangent = True): ''' Camera calibration searches through all the images (jpg or png) located in _path_ for matches to a checkerboard pattern of size checkboardSize. These images should all be of the same camera with the same resolution. @@ -491,10 +488,18 @@ if len(objpoints) == 0 or len(imgpoints) == 0: return None try: - ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None) + flags = 0 + if fixK2: + flags += cv2.cv.CV_CALIB_FIX_K2 + if fixK3: + flags += cv2.cv.CV_CALIB_FIX_K3 + if zeroTangent: + flags += cv2.cv.CV_CALIB_ZERO_TANGENT_DIST + ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None, flags = flags) except NameError: return None savetxt('intrinsic-camera.txt', camera_matrix) + print 'error: {}'.format(ret) return camera_matrix, dist_coeffs def undistortImage(img, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1., interpolation=cv2.INTER_LINEAR):
--- a/scripts/undistort-video.py Mon Jul 10 01:38:12 2017 -0400 +++ b/scripts/undistort-video.py Mon Jul 10 18:04:41 2017 -0400 @@ -16,6 +16,7 @@ parser.add_argument('--intrinsic', dest = 'intrinsicCameraMatrixFilename', help = 'name of the intrinsic camera file') parser.add_argument('--distortion-coefficients', dest = 'distortionCoefficients', help = 'distortion coefficients', nargs = '*', type = float) parser.add_argument('--undistorted-multiplication', dest = 'undistortedImageMultiplication', help = 'undistorted image multiplication', type = float) +parser.add_argument('--mask', dest = 'maskFilename', help = 'name of the mask file, to undistort to see how it covers the undistortion errors') parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', type = int, default = 0) parser.add_argument('-l', dest = 'lastFrameNum', help = 'number of last frame number to save', type = int) parser.add_argument('-d', dest = 'destinationDirname', help = 'name of the directory where the undistorted frames are saved') @@ -26,9 +27,6 @@ args = parser.parse_args() intrinsicCameraMatrix = np.loadtxt(args.intrinsicCameraMatrixFilename) -#distortionCoefficients = args.distortionCoefficients -#undistortedImageMultiplication = args.undistortedImageMultiplication -#firstFrameNum = params.firstFrameNum if args.destinationDirname is None: destinationDirname = '' else: @@ -43,6 +41,9 @@ width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)) height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)) [map1, map2] = cvutils.computeUndistortMaps(width, height, args.undistortedImageMultiplication, intrinsicCameraMatrix, args.distortionCoefficients) +if args.maskFilename is not None: + mask = cv2.imread(args.maskFilename) + undistortedMask = cv2.remap(mask, map1, map2, interpolation=cv2.INTER_LINEAR)/255 if capture.isOpened(): ret = True @@ -59,6 +60,8 @@ if ret: img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR) cv2.imwrite(destinationDirname+'undistorted-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), img) + if args.maskFilename is not None: + cv2.imwrite(destinationDirname+'undistorted+mask-{{:0{}}}.png'.format(nZerosFilename).format(frameNum), cv2.multiply(img, undistortedMask)) frameNum += 1 if args.encodeVideo: