From aafda43df12207be7c3981c7a7ac91309977f042 Mon Sep 17 00:00:00 2001 From: PhilLab Date: Tue, 8 Jul 2014 11:52:42 +0200 Subject: [PATCH 1/3] Double precision for solvePnPRansac() solvePnPRansac() and pnpTask() now accept object or image points with double precision. --- modules/calib3d/src/solvepnp.cpp | 43 ++++++++++++++++++++++++++------ 1 file changed, 35 insertions(+), 8 deletions(-) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index b0ef1d9830..2b4552463f 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -137,11 +137,13 @@ namespace cv CameraParameters camera; }; + template static void pnpTask(const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, const Parameters& params, vector& inliers, Mat& rvec, Mat& tvec, const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) { - Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_32FC3), modelImagePoints(1, MIN_POINTS_COUNT, CV_32FC2); + Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth::value, 3)); + Mat modelImagePoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(DataDepth::value, 2)); for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++) { if (pointsMask[i]) @@ -160,7 +162,7 @@ namespace cv for (int i = 0; i < MIN_POINTS_COUNT; i++) for (int j = i + 1; j < MIN_POINTS_COUNT; j++) { - if (norm(modelObjectPoints.at(0, i) - modelObjectPoints.at(0, j)) < eps) + if (norm(modelObjectPoints.at>(0, i) - modelObjectPoints.at>(0, j)) < eps) num_same_points++; } if (num_same_points > 0) @@ -174,7 +176,7 @@ namespace cv params.useExtrinsicGuess, params.flags); - vector projected_points; + vector> projected_points; projected_points.resize(objectPoints.cols); projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points); @@ -184,9 +186,10 @@ namespace cv vector localInliers; for (int i = 0; i < objectPoints.cols; i++) { - Point2f p(imagePoints.at(0, i)[0], imagePoints.at(0, i)[1]); + //Although p is a 2D point it needs the same type as the object points to enable the norm calculation + Point_ p(imagePoints.at>(0, i)[0], imagePoints.at>(0, i)[1]); if ((norm(p - projected_points[i]) < params.reprojectionError) - && (rotatedPoints.at(0, i)[2] > 0)) //hack + && (rotatedPoints.at>(0, i)[2] > 0)) //hack { localInliers.push_back(i); } @@ -206,6 +209,30 @@ namespace cv } } + static void pnpTask(const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, + const Parameters& params, vector& inliers, Mat& rvec, Mat& tvec, + const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) + { + CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F); + CV_Assert(imagePoints.depth() == CV_64F || imagePoints.depth() == CV_32F); + const bool objectDoublePrecision = objectPoints.depth() == CV_64F; + const bool imageDoublePrecision = imagePoints.depth() == CV_64F; + if(objectDoublePrecision) + { + if(imageDoublePrecision) + pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + else + pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + } + else + { + if(imageDoublePrecision) + pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + else + pnpTask(pointsMask, objectPoints, imagePoints, params, inliers, rvec, tvec, rvecInit, tvecInit, resultsMutex); + } + } + class PnPSolver { public: @@ -281,10 +308,10 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat(); CV_Assert(opoints.isContinuous()); - CV_Assert(opoints.depth() == CV_32F); + CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F); CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3); CV_Assert(ipoints.isContinuous()); - CV_Assert(ipoints.depth() == CV_32F); + CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F); CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2); _rvec.create(3, 1, CV_64FC1); @@ -320,7 +347,7 @@ void cv::solvePnPRansac(InputArray _opoints, InputArray _ipoints, if (flags != CV_P3P) { int i, pointsCount = (int)localInliers.size(); - Mat inlierObjectPoints(1, pointsCount, CV_32FC3), inlierImagePoints(1, pointsCount, CV_32FC2); + Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2)); for (i = 0; i < pointsCount; i++) { int index = localInliers[i]; From 2c29ee9e00c204f0c4b3b3685485c4c9b0350650 Mon Sep 17 00:00:00 2001 From: PhilLab Date: Tue, 8 Jul 2014 13:24:35 +0200 Subject: [PATCH 2/3] Added cast and removed formatting error --- modules/calib3d/src/solvepnp.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 2b4552463f..081d6bd5c8 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -187,7 +187,8 @@ namespace cv for (int i = 0; i < objectPoints.cols; i++) { //Although p is a 2D point it needs the same type as the object points to enable the norm calculation - Point_ p(imagePoints.at>(0, i)[0], imagePoints.at>(0, i)[1]); + Point_ p((OpointType)imagePoints.at>(0, i)[0], + (OpointType)imagePoints.at>(0, i)[1]); if ((norm(p - projected_points[i]) < params.reprojectionError) && (rotatedPoints.at>(0, i)[2] > 0)) //hack { @@ -212,9 +213,9 @@ namespace cv static void pnpTask(const vector& pointsMask, const Mat& objectPoints, const Mat& imagePoints, const Parameters& params, vector& inliers, Mat& rvec, Mat& tvec, const Mat& rvecInit, const Mat& tvecInit, Mutex& resultsMutex) - { - CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F); - CV_Assert(imagePoints.depth() == CV_64F || imagePoints.depth() == CV_32F); + { + CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F); + CV_Assert(imagePoints.depth() == CV_64F || imagePoints.depth() == CV_32F); const bool objectDoublePrecision = objectPoints.depth() == CV_64F; const bool imageDoublePrecision = imagePoints.depth() == CV_64F; if(objectDoublePrecision) From 52c05e75cc1987575d7287ece117ec0c321638c1 Mon Sep 17 00:00:00 2001 From: unknown Date: Wed, 9 Jul 2014 14:19:15 +0200 Subject: [PATCH 3/3] Fixed C++11 compatibility warning --- modules/calib3d/src/solvepnp.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 081d6bd5c8..1aeb82f64b 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -162,7 +162,7 @@ namespace cv for (int i = 0; i < MIN_POINTS_COUNT; i++) for (int j = i + 1; j < MIN_POINTS_COUNT; j++) { - if (norm(modelObjectPoints.at>(0, i) - modelObjectPoints.at>(0, j)) < eps) + if (norm(modelObjectPoints.at >(0, i) - modelObjectPoints.at >(0, j)) < eps) num_same_points++; } if (num_same_points > 0) @@ -176,7 +176,7 @@ namespace cv params.useExtrinsicGuess, params.flags); - vector> projected_points; + vector > projected_points; projected_points.resize(objectPoints.cols); projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points); @@ -187,10 +187,10 @@ namespace cv for (int i = 0; i < objectPoints.cols; i++) { //Although p is a 2D point it needs the same type as the object points to enable the norm calculation - Point_ p((OpointType)imagePoints.at>(0, i)[0], - (OpointType)imagePoints.at>(0, i)[1]); + Point_ p((OpointType)imagePoints.at >(0, i)[0], + (OpointType)imagePoints.at >(0, i)[1]); if ((norm(p - projected_points[i]) < params.reprojectionError) - && (rotatedPoints.at>(0, i)[2] > 0)) //hack + && (rotatedPoints.at >(0, i)[2] > 0)) //hack { localInliers.push_back(i); }