Mercurial Hosting > traffic-intelligence
changeset 933:8ac7f61c6e4f
major rework of homography calibration, no in ideal points if correcting for distortion
| author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
|---|---|
| date | Fri, 14 Jul 2017 02:11:21 -0400 |
| parents | 66f382852e61 |
| children | 39691b460fca |
| files | c/Motion.cpp c/cvutils.cpp c/feature-based-tracking.cpp include/Motion.hpp include/cvutils.hpp python/cvutils.py python/moving.py python/tests/cvutils.txt scripts/compute-homography.py |
| diffstat | 9 files changed, 49 insertions(+), 63 deletions(-) [+] |
line wrap: on
line diff
--- a/c/Motion.cpp Fri Jul 14 00:12:03 2017 -0400 +++ b/c/Motion.cpp Fri Jul 14 02:11:21 2017 -0400 @@ -125,17 +125,21 @@ #ifdef USE_OPENCV /// \todo add option for anti-aliased drawing, thickness -void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Scalar& color) const { +void FeatureTrajectory::draw(Mat& img, const Mat& homography, const Mat& intrinsicCameraMatrix, const Scalar& color) const { Point2f p1, p2; if (!homography.empty()) p1 = project((*positions)[0], homography); else p1 = (*positions)[0]; + if (!intrinsicCameraMatrix.empty()) + p1 = cameraProject(p1, intrinsicCameraMatrix); for (unsigned int i=1; i<positions->size(); i++) { if (!homography.empty()) p2 = project((*positions)[i], homography); else p2 = (*positions)[i]; + if (!intrinsicCameraMatrix.empty()) + p2 = cameraProject(p2, intrinsicCameraMatrix); line(img, p1, p2, color, 1); p1 = p2; }
--- a/c/cvutils.cpp Fri Jul 14 00:12:03 2017 -0400 +++ b/c/cvutils.cpp Fri Jul 14 02:11:21 2017 -0400 @@ -25,6 +25,14 @@ return Point2f(x, y); } +Point2f cameraProject(const Point2f& p, const Mat& cameraMatrix) { + //Mat homogeneous(3, 1, CV_32FC1); + float x, y; + x = cameraMatrix.at<double>(0,0)*p.x+cameraMatrix.at<double>(0,2); + y = cameraMatrix.at<double>(1,1)*p.y+cameraMatrix.at<double>(1,2); + 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 Fri Jul 14 00:12:03 2017 -0400 +++ b/c/feature-based-tracking.cpp Fri Jul 14 02:11:21 2017 -0400 @@ -115,7 +115,7 @@ if (params.undistort) { intrinsicCameraMatrix = ::loadMat(params.intrinsicCameraFilename, " "); 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); + newIntrinsicCameraMatrix = getDefaultNewCameraMatrix(intrinsicCameraMatrix, undistortedVideoSize, true);// for some reason, it's double type //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); cout << "Undistorted width=" << undistortedVideoSize.width << @@ -167,7 +167,7 @@ /// \todo try calcOpticalFlowFarneback if (params.undistort) - undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); + undistortPoints(currPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients); else undistortedPts =currPts; @@ -208,15 +208,14 @@ currPts = trackedPts; assert(currPts.size() == featurePointMatches.size()); saveFeatures(lostFeatures, *trajectoryDB, "positions", "velocities"); - + if (params.display) { if (params.undistort) remap(frame, displayFrame, map1, map2, interpolationMethod, BORDER_CONSTANT, 0.); else displayFrame = frame.clone(); - BOOST_FOREACH(FeaturePointMatch fp, featurePointMatches) - fp.feature->draw(displayFrame, invHomography, Colors::red()); + fp.feature->draw(displayFrame, invHomography, newIntrinsicCameraMatrix, Colors::red()); } } @@ -228,7 +227,7 @@ 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) - undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients, noArray(), newIntrinsicCameraMatrix); + undistortPoints(newPts, undistortedPts, intrinsicCameraMatrix, params.distortionCoefficients); else undistortedPts = newPts;
--- a/include/Motion.hpp Fri Jul 14 00:12:03 2017 -0400 +++ b/include/Motion.hpp Fri Jul 14 02:11:21 2017 -0400 @@ -52,7 +52,7 @@ void write(TrajectoryDBAccess<cv::Point2f>& trajectoryDB, const std::string& positionsTableName, const std::string& velocitiesTableName) const; #ifdef USE_OPENCV - void draw(cv::Mat& img, const cv::Mat& homography, const cv::Scalar& color) const; + void draw(cv::Mat& img, const cv::Mat& homography, const cv::Mat& intrinsicCameraMatrix, const cv::Scalar& color) const; #endif friend std::ostream& operator<<(std::ostream& out, const FeatureTrajectory& ft);
--- a/include/cvutils.hpp Fri Jul 14 00:12:03 2017 -0400 +++ b/include/cvutils.hpp Fri Jul 14 02:11:21 2017 -0400 @@ -14,6 +14,9 @@ use perspectiveTransform for arrays of points. */ cv::Point2f project(const cv::Point2f& p, const cv::Mat& homography); +/** Projects a point with the camera matrix */ +cv::Point2f cameraProject(const cv::Point2f& p, const cv::Mat& cameraMatrix); + /** 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);
--- a/python/cvutils.py Fri Jul 14 00:12:03 2017 -0400 +++ b/python/cvutils.py Fri Jul 14 02:11:21 2017 -0400 @@ -124,7 +124,7 @@ def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients): newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication))) newCameraMatrix = cv2.getDefaultNewCameraMatrix(intrinsicCameraMatrix, newImgSize, True) - return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), None, newCameraMatrix, newImgSize, cv2.CV_32FC1) + return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), None, newCameraMatrix, newImgSize, cv2.CV_32FC1), newCameraMatrix def playVideo(filenames, windowNames = None, firstFrameNums = None, frameRate = -1, interactive = False, printFrames = True, text = None, rescale = 1., step = 1, colorBlind = False): '''Plays the video(s)''' @@ -305,7 +305,7 @@ cv2.namedWindow(windowName, cv2.WINDOW_NORMAL) if undistort: # setup undistortion - [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) + [map1, map2], newCameraMatrix = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients) if capture.isOpened(): key = -1 ret = True @@ -330,10 +330,10 @@ for obj in objects: if obj.existsAtInstant(frameNum): if not hasattr(obj, 'projectedPositions'): - if homography is not None: - obj.projectedPositions = obj.positions.project(homography) - else: - obj.projectedPositions = obj.positions + obj.projectedPositions = obj.getPositions().homographyProject(homography) + if undistort: + obj.projectedPositions = obj.projectedPositions.newCameraProject(newCameraMatrix) + #obj.projectedPositions = obj.positions cvPlot(img, obj.projectedPositions, cvColors[colorType][obj.getNum()], frameNum-obj.getFirstInstant()) if frameNum not in boundingBoxes.keys() and obj.hasFeatures(): yCropMin, yCropMax, xCropMin, xCropMax = imageBoxSize(obj, frameNum, homography, width, height)
--- a/python/moving.py Fri Jul 14 00:12:03 2017 -0400 +++ b/python/moving.py Fri Jul 14 02:11:21 2017 -0400 @@ -742,9 +742,12 @@ plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs) if objNum is not None: text(positions[0][0], positions[1][0], '{}'.format(objNum)) - - def project(self, homography): - return Trajectory(cvutils.projectTrajectory(homography, self.positions).tolist()) + + def homographyProject(self, homography): + return Trajectory(cvutils.homographyProject(array(self.positions), homography).tolist()) + + def newCameraProject(self, newCameraMatrix): + return Trajectory(cvutils.newCameraProject(array(self.positions), newCameraMatrix).tolist()) def plot(self, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs): Trajectory._plot(self.positions, options, withOrigin, None, timeStep, objNum, **kwargs)
--- a/python/tests/cvutils.txt Fri Jul 14 00:12:03 2017 -0400 +++ b/python/tests/cvutils.txt Fri Jul 14 02:11:21 2017 -0400 @@ -6,7 +6,7 @@ >>> intrinsicCameraMatrix = array([[ 377.42, 0. , 639.12], [ 0. , 378.43, 490.2 ], [ 0. , 0. , 1. ]]) >>> distortionCoefficients = array([-0.11759321, 0.0148536, 0.00030756, -0.00020578, -0.00091816])# distortionCoefficients = array([-0.11759321, 0., 0., 0., 0.]) >>> multiplicationFactor = 1.31 ->>> [map1, map2] = cvutils.computeUndistortMaps(width, height, multiplicationFactor, intrinsicCameraMatrix, distortionCoefficients) +>>> [map1, map2], tmp = cvutils.computeUndistortMaps(width, height, multiplicationFactor, intrinsicCameraMatrix, distortionCoefficients) >>> undistorted = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR) >>> (undistorted.shape == array([int(round(height*multiplicationFactor)), int(round(width*multiplicationFactor)), 3])).all() True @@ -26,6 +26,11 @@ >>> (reducedPoints == reducedPoints).all() True +>>> undistortedPoints2 = cv2.undistortPoints(imgPoints, intrinsicCameraMatrix, distortionCoefficients).reshape(-1, 2) # undistort and project as if seen by new camera +>>> undistortedPoints2 = cvutils.newCameraProject(undistortedPoints2.T, newCameraMatrix) +>>> (undistortedPoints == undistortedPoints2.T).all() +True + >>> undistortedPoints = cv2.undistortPoints(imgPoints, intrinsicCameraMatrix, distortionCoefficients).reshape(-1, 2) # undistort to ideal points >>> origPoints = cvutils.worldToImageProject(undistortedPoints.T, intrinsicCameraMatrix, distortionCoefficients).T >>> (round(origPoints[1:,:]) == imgPoints[0][1:,:]).all()
--- a/scripts/compute-homography.py Fri Jul 14 00:12:03 2017 -0400 +++ b/scripts/compute-homography.py Fri Jul 14 02:11:21 2017 -0400 @@ -36,47 +36,6 @@ args = parser.parse_args() -# TODO process camera intrinsic and extrinsic parameters to obtain image to world homography, taking example from Work/src/python/generate-homography.py script -# cameraMat = load(videoFilenamePrefix+'-camera.txt'); -# T1 = cameraMat[3:6,:].copy(); -# A = cameraMat[0:3,0:3].copy(); - -# # pay attention, rotation may be the transpose -# # R = T1[:,0:3].T; -# R = T1[:,0:3]; -# rT = dot(R, T1[:,3]/1000); -# T = zeros((3,4),'f'); -# T[:,0:3] = R[:]; -# T[:,3] = rT; - -# AT = dot(A,T); - -# nPoints = 4; -# worldPoints = cvCreateMat(nPoints, 3, CV_64FC1); -# imagePoints = cvCreateMat(nPoints, 3, CV_64FC1); - -# # extract homography from the camera calibration -# worldPoints = cvCreateMat(4, 3, CV_64FC1); -# imagePoints = cvCreateMat(4, 3, CV_64FC1); - -# worldPoints[0,:] = [[1, 1, 0]]; -# worldPoints[1,:] = [[1, 2, 0]]; -# worldPoints[2,:] = [[2, 1, 0]]; -# worldPoints[3,:] = [[2, 2, 0]]; - -# wPoints = [[1,1,2,2], -# [1,2,1,2], -# [0,0,0,0]]; -# iPoints = utils.worldToImage(AT, wPoints); - -# for i in range(nPoints): -# imagePoints[i,:] = [iPoints[:,i].tolist()]; - -# H = cvCreateMat(3, 3, CV_64FC1); - -# cvFindHomography(imagePoints, worldPoints, H); - - homography = np.array([]) if args.pointCorrespondencesFilename is not None: worldPts, videoPts = cvutils.loadPointCorrespondences(args.pointCorrespondencesFilename) @@ -90,13 +49,15 @@ worldImg = plt.imread(args.worldFilename) videoImg = plt.imread(args.videoFrameFilename) if args.undistort: - [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients) + [map1, map2], newCameraMatrix = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients) videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR) print('Click on {} points in the video frame'.format(args.nPoints)) plt.figure() plt.imshow(videoImg) plt.tight_layout() videoPts = np.array(plt.ginput(args.nPoints, timeout=3000)) + if args.undistort: + videoPts = cvutils.newCameraProject(videoPts, np.linalg.inv(newCameraMatrix)) print('Click on {} points in the world image'.format(args.nPoints)) plt.figure() plt.imshow(worldImg) @@ -117,13 +78,16 @@ worldImg = cv2.imread(args.worldFilename) videoImg = cv2.imread(args.videoFrameFilename) if args.undistort: - [map1, map2] = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients) + [map1, map2], newCameraMatrix = cvutils.computeUndistortMaps(videoImg.shape[1], videoImg.shape[0], args.undistortedImageMultiplication, np.loadtxt(args.intrinsicCameraMatrixFilename), args.distortionCoefficients) videoImg = cv2.remap(videoImg, map1, map2, interpolation=cv2.INTER_LINEAR) if args.saveImages: cv2.imwrite(utils.removeExtension(args.videoFrameFilename)+'-undistorted.png', videoImg) invHomography = np.linalg.inv(homography) - projectedWorldPts = cvutils.projectArray(invHomography, worldPts.T).T - projectedVideoPts = cvutils.projectArray(homography, videoPts.T).T + projectedWorldPts = cvutils.homographyProject(worldPts.T, invHomography).T + projectedVideoPts = cvutils.homographyProject(videoPts.T, homography).T + if args.undistort: + projectedWorldPts = cvutils.newCameraProject(projectedWorldPts.T, newCameraMatrix).T + videoPts = cvutils.newCameraProject(videoPts.T, newCameraMatrix).T for i in range(worldPts.shape[0]): # world image cv2.circle(worldImg,tuple(np.int32(np.round(worldPts[i]/args.unitsPerPixel))),2,cvutils.cvBlue['default'])