From 0c9c7d5f3b79218edf2f4352e7ffb574e7f81483 Mon Sep 17 00:00:00 2001 From: Alexander Karsakov Date: Tue, 12 Jan 2016 12:13:28 +0300 Subject: [PATCH 1/2] Added test for checking that solvePnPRansac accepts vector and Nx3 1-channel or 1xN/Nx1 3-channel matrices as input. --- modules/calib3d/src/solvepnp.cpp | 2 + modules/calib3d/test/test_solvepnp_ransac.cpp | 37 +++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index 8b20cf7e46..23575199d9 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/modules/calib3d/src/solvepnp.cpp @@ -270,6 +270,8 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, { vector opoints_inliers; vector ipoints_inliers; + opoints = opoints.reshape(3); + ipoints = ipoints.reshape(2); opoints.convertTo(opoints_inliers, CV_64F); ipoints.convertTo(ipoints_inliers, CV_64F); diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 5e3dbc60db..579248893a 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -314,6 +314,43 @@ TEST(Calib3d_SolvePnPRansac, concurrency) EXPECT_LT(tnorm, 1e-6); } +TEST(Calib3d_SolvePnPRansac, input_type) +{ + const int numPoints = 10; + Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., + 5.4817724002728005e+002, 2.3062194051986233e+002, 0., 0., 1.); + + std::vector points3d; + std::vector points2d; + for (int i = 0; i < numPoints; i++) + { + points3d.push_back(cv::Point3i(i, 0, 0)); + points2d.push_back(cv::Point2i(i, 0)); + } + Mat R1, t1, R2, t2, R3, t3, R4, t4; + + ASSERT_TRUE(solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R1, t1)); + + Mat points3dMat(points3d); + Mat points2dMat(points2d); + ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R2, t2)); + + points3dMat = points3dMat.reshape(3, 1); + points2dMat = points2dMat.reshape(2, 1); + ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R3, t3)); + + points3dMat = points3dMat.reshape(1, numPoints); + points2dMat = points2dMat.reshape(1, numPoints); + ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R4, t4)); + + ASSERT_LE(norm(R1, R2, NORM_INF), 1e-6); + ASSERT_LE(norm(t1, t2, NORM_INF), 1e-6); + ASSERT_LE(norm(R1, R3, NORM_INF), 1e-6); + ASSERT_LE(norm(t1, t3, NORM_INF), 1e-6); + ASSERT_LE(norm(R1, R4, NORM_INF), 1e-6); + ASSERT_LE(norm(t1, t4, NORM_INF), 1e-6); +} + TEST(Calib3d_SolvePnP, double_support) { Matx33d intrinsics(5.4794130238156129e+002, 0., 2.9835545700043139e+002, 0., From e784ea7178047a1e4f9b010471703e1c2974cda1 Mon Sep 17 00:00:00 2001 From: Alexander Karsakov Date: Tue, 12 Jan 2016 12:14:19 +0300 Subject: [PATCH 2/2] Fixed getSubset method to support Nx3 1-channel matrices as input (3xN 1-channel matrices doesn't supported at all). --- modules/calib3d/include/opencv2/calib3d.hpp | 8 ++-- modules/calib3d/src/ptsetreg.cpp | 2 +- modules/calib3d/test/test_solvepnp_ransac.cpp | 38 +++++++++---------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index 867bbc723c..b1d819f022 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -515,9 +515,9 @@ CV_EXPORTS_W void projectPoints( InputArray objectPoints, /** @brief Finds an object pose from 3D-2D point correspondences. -@param objectPoints Array of object points in the object coordinate space, 3xN/Nx3 1-channel or +@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector\ can be also passed here. -@param imagePoints Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, +@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. vector\ can be also passed here. @param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ . @param distCoeffs Input vector of distortion coefficients @@ -572,9 +572,9 @@ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, /** @brief Finds an object pose from 3D-2D point correspondences using the RANSAC scheme. -@param objectPoints Array of object points in the object coordinate space, 3xN/Nx3 1-channel or +@param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points. vector\ can be also passed here. -@param imagePoints Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, +@param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points. vector\ can be also passed here. @param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ . @param distCoeffs Input vector of distortion coefficients diff --git a/modules/calib3d/src/ptsetreg.cpp b/modules/calib3d/src/ptsetreg.cpp index 75a73a5cb7..e83e19a203 100644 --- a/modules/calib3d/src/ptsetreg.cpp +++ b/modules/calib3d/src/ptsetreg.cpp @@ -109,9 +109,9 @@ public: cv::AutoBuffer _idx(modelPoints); int* idx = _idx; int i = 0, j, k, iters = 0; - int esz1 = (int)m1.elemSize(), esz2 = (int)m2.elemSize(); int d1 = m1.channels() > 1 ? m1.channels() : m1.cols; int d2 = m2.channels() > 1 ? m2.channels() : m2.cols; + int esz1 = (int)m1.elemSize1()*d1, esz2 = (int)m2.elemSize1()*d2; int count = m1.checkVector(d1), count2 = m2.checkVector(d2); const int *m1ptr = m1.ptr(), *m2ptr = m2.ptr(); diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 579248893a..4efdc79902 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -329,26 +329,26 @@ TEST(Calib3d_SolvePnPRansac, input_type) } Mat R1, t1, R2, t2, R3, t3, R4, t4; - ASSERT_TRUE(solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R1, t1)); + EXPECT_TRUE(solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R1, t1)); Mat points3dMat(points3d); Mat points2dMat(points2d); - ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R2, t2)); + EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R2, t2)); points3dMat = points3dMat.reshape(3, 1); points2dMat = points2dMat.reshape(2, 1); - ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R3, t3)); + EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R3, t3)); points3dMat = points3dMat.reshape(1, numPoints); points2dMat = points2dMat.reshape(1, numPoints); - ASSERT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R4, t4)); - - ASSERT_LE(norm(R1, R2, NORM_INF), 1e-6); - ASSERT_LE(norm(t1, t2, NORM_INF), 1e-6); - ASSERT_LE(norm(R1, R3, NORM_INF), 1e-6); - ASSERT_LE(norm(t1, t3, NORM_INF), 1e-6); - ASSERT_LE(norm(R1, R4, NORM_INF), 1e-6); - ASSERT_LE(norm(t1, t4, NORM_INF), 1e-6); + EXPECT_TRUE(solvePnPRansac(points3dMat, points2dMat, intrinsics, cv::Mat(), R4, t4)); + + EXPECT_LE(norm(R1, R2, NORM_INF), 1e-6); + EXPECT_LE(norm(t1, t2, NORM_INF), 1e-6); + EXPECT_LE(norm(R1, R3, NORM_INF), 1e-6); + EXPECT_LE(norm(t1, t3, NORM_INF), 1e-6); + EXPECT_LE(norm(R1, R4, NORM_INF), 1e-6); + EXPECT_LE(norm(t1, t4, NORM_INF), 1e-6); } TEST(Calib3d_SolvePnP, double_support) @@ -372,8 +372,8 @@ TEST(Calib3d_SolvePnP, double_support) solvePnPRansac(points3dF, points2dF, intrinsics, cv::Mat(), RF, tF, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P); solvePnPRansac(points3d, points2d, intrinsics, cv::Mat(), R, t, true, 100, 8.f, 0.999, inliers, cv::SOLVEPNP_P3P); - ASSERT_LE(norm(R, Mat_(RF), NORM_INF), 1e-3); - ASSERT_LE(norm(t, Mat_(tF), NORM_INF), 1e-3); + EXPECT_LE(norm(R, Mat_(RF), NORM_INF), 1e-3); + EXPECT_LE(norm(t, Mat_(tF), NORM_INF), 1e-3); } TEST(Calib3d_SolvePnP, translation) @@ -402,16 +402,16 @@ TEST(Calib3d_SolvePnP, translation) tvec = (Mat_(3,1) << 100, 100, 0); solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true); - ASSERT_TRUE(checkRange(rvec)); - ASSERT_TRUE(checkRange(tvec)); + EXPECT_TRUE(checkRange(rvec)); + EXPECT_TRUE(checkRange(tvec)); rvec =(Mat_(3,1) << 0, 0, 0); tvec = (Mat_(3,1) << 100, 100, 0); solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, true); - ASSERT_TRUE(checkRange(rvec)); - ASSERT_TRUE(checkRange(tvec)); + EXPECT_TRUE(checkRange(rvec)); + EXPECT_TRUE(checkRange(tvec)); solvePnP(p3d, p2d, cameraIntrinsic, noArray(), rvec, tvec, false); - ASSERT_TRUE(checkRange(rvec)); - ASSERT_TRUE(checkRange(tvec)); + EXPECT_TRUE(checkRange(rvec)); + EXPECT_TRUE(checkRange(tvec)); }