diff --git a/modules/calib3d/src/usac.hpp b/modules/calib3d/src/usac.hpp index 06a0ff2056..6dc79fdc55 100644 --- a/modules/calib3d/src/usac.hpp +++ b/modules/calib3d/src/usac.hpp @@ -116,7 +116,7 @@ public: class P3PSolver : public MinimalSolver { public: - static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); + static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K); }; //-------------------------- AFFINE ----------------------- @@ -164,7 +164,7 @@ public: class DLSPnP : public NonMinimalSolver { public: - static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); + static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K); }; //-------------------------- AFFINE ----------------------- @@ -571,11 +571,11 @@ namespace Utils { * @points is matrix N x 4. * @norm_points is output matrix N x 4 with calibrated points. */ - void calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &norm_points); - void calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts); - void normalizeAndDecalibPointsPnP (const Mat &K, Mat &pts, Mat &calib_norm_pts); - void decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal=false); - double getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2); + void calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &norm_points); + void calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts); + void normalizeAndDecalibPointsPnP (const Matx33d &K, Mat &pts, Mat &calib_norm_pts); + void decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal=false); + double getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2); float findMedian (std::vector &array); } namespace Math { diff --git a/modules/calib3d/src/usac/dls_solver.cpp b/modules/calib3d/src/usac/dls_solver.cpp index 0898734fb3..0abb26cecc 100644 --- a/modules/calib3d/src/usac/dls_solver.cpp +++ b/modules/calib3d/src/usac/dls_solver.cpp @@ -49,13 +49,14 @@ namespace cv { namespace usac { // This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented class DLSPnPImpl : public DLSPnP { private: - const Mat * points_mat, * calib_norm_points_mat, * K_mat; + const Mat * points_mat, * calib_norm_points_mat; + const Matx33d * K_mat; #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN) - const Mat &K; + const Matx33d &K; const float * const calib_norm_points, * const points; #endif public: - explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_) : + explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) : points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_) #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN) , K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data) @@ -870,7 +871,7 @@ protected: 2 * D[74] - 2 * D[78]); // s1^3 } }; -Ptr DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) { +Ptr DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) { return makePtr(points_, calib_norm_pts, K); } }} diff --git a/modules/calib3d/src/usac/pnp_solver.cpp b/modules/calib3d/src/usac/pnp_solver.cpp index e095eeff7c..37c649144c 100644 --- a/modules/calib3d/src/usac/pnp_solver.cpp +++ b/modules/calib3d/src/usac/pnp_solver.cpp @@ -263,7 +263,8 @@ private: * calibrated normalized points * K^-1 [u v 1]^T / ||K^-1 [u v 1]^T|| */ - const Mat * points_mat, * calib_norm_points_mat, * K_mat, &K; + const Mat * points_mat, * calib_norm_points_mat; + const Matx33d * K_mat, &K; const float * const calib_norm_points, * const points; const double VAL_THR = 1e-4; public: @@ -271,7 +272,7 @@ public: * @points_ is matrix N x 5 * u v x y z. (u,v) is image point, (x y z) is world point */ - P3PSolverImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_) : + P3PSolverImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) : points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_), K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data) {} @@ -362,7 +363,7 @@ public: const Matx33d R = Math::rotVec2RotMat(Math::rotMat2RotVec(Z * Zw.inv())); - Mat P, KR = K * R; + Mat P, KR = Mat(K * R); hconcat(KR, -KR * (X1 - R.t() * nX1), P); models.emplace_back(P); } @@ -374,7 +375,7 @@ public: return makePtr(*points_mat, *calib_norm_points_mat, *K_mat); } }; -Ptr P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) { +Ptr P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) { return makePtr(points_, calib_norm_pts, K); } }} \ No newline at end of file diff --git a/modules/calib3d/src/usac/ransac_solvers.cpp b/modules/calib3d/src/usac/ransac_solvers.cpp index b7f3e6e0c1..fe64907ec0 100644 --- a/modules/calib3d/src/usac/ransac_solvers.cpp +++ b/modules/calib3d/src/usac/ransac_solvers.cpp @@ -774,13 +774,14 @@ bool run (const Ptr ¶ms, InputArray points1, InputArray points2 Ptr min_solver; Ptr non_min_solver; - Mat points, K1, K2, calib_points, undist_points1, undist_points2; + Mat points, calib_points, undist_points1, undist_points2; + Matx33d K1, K2; int points_size; double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold(); const int min_sample_size = params->getSampleSize(); if (params->isPnP()) { if (! K1_.empty()) { - K1 = K1_.getMat(); K1.convertTo(K1, CV_64F); + K1 = K1_.getMat(); if (! dist_coeff1.empty()) { // undistortPoints also calibrate points using K if (points1.isContinuous()) @@ -797,8 +798,8 @@ bool run (const Ptr ¶ms, InputArray points1, InputArray points2 } else { if (params->isEssential()) { CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!"); - K1 = K1_.getMat(); K1.convertTo(K1, CV_64F); - K2 = K2_.getMat(); K2.convertTo(K2, CV_64F); + K1 = K1_.getMat(); + K2 = K2_.getMat(); if (! dist_coeff1.empty() || ! dist_coeff2.empty()) { // undistortPoints also calibrate points using K if (points1.isContinuous()) @@ -1011,7 +1012,7 @@ bool run (const Ptr ¶ms, InputArray points1, InputArray points2 // convert R to rodrigues and back and recalculate inliers which due to numerical // issues can differ Mat out, R, newR, newP, t, rvec; - if (K1.empty()) { + if (K1_.empty()) { usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t); Rodrigues(R, rvec); hconcat(rvec, t, out); diff --git a/modules/calib3d/src/usac/utils.cpp b/modules/calib3d/src/usac/utils.cpp index 1c781a7d82..c4d74eb663 100644 --- a/modules/calib3d/src/usac/utils.cpp +++ b/modules/calib3d/src/usac/utils.cpp @@ -8,9 +8,9 @@ #include namespace cv { namespace usac { -double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2) { - return threshold / ((K1.at(0, 0) + K1.at(1, 1) + - K2.at(0, 0) + K2.at(1, 1)) / 4.0); +double Utils::getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2) { + return threshold / ((K1(0, 0) + K1(1, 1) + + K2(0, 0) + K2(1, 1)) / 4.0); } /* @@ -20,22 +20,20 @@ double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat * 0 k22 k23 * 0 0 1] */ -void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &calib_points) { - const auto * const points_ = (float *) points.data; - const auto * const k1 = (double *) K1.data; - const auto inv1_k11 = float(1 / k1[0]); // 1 / k11 - const auto inv1_k12 = float(-k1[1] / (k1[0]*k1[4])); // -k12 / (k11*k22) +void Utils::calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &calib_points) { + const auto inv1_k11 = float(1 / K1(0, 0)); // 1 / k11 + const auto inv1_k12 = float(-K1(0, 1) / (K1(0, 0)*K1(1, 1))); // -k12 / (k11*k22) // (-k13*k22 + k12*k23) / (k11*k22) - const auto inv1_k13 = float((-k1[2]*k1[4] + k1[1]*k1[5]) / (k1[0]*k1[4])); - const auto inv1_k22 = float(1 / k1[4]); // 1 / k22 - const auto inv1_k23 = float(-k1[5] / k1[4]); // -k23 / k22 + const auto inv1_k13 = float((-K1(0, 2)*K1(1, 1) + K1(0, 1)*K1(1, 2)) / (K1(0, 0)*K1(1, 1))); + const auto inv1_k22 = float(1 / K1(1, 1)); // 1 / k22 + const auto inv1_k23 = float(-K1(1, 2) / K1(1, 1)); // -k23 / k22 - const auto * const k2 = (double *) K2.data; - const auto inv2_k11 = float(1 / k2[0]); - const auto inv2_k12 = float(-k2[1] / (k2[0]*k2[4])); - const auto inv2_k13 = float((-k2[2]*k2[4] + k2[1]*k2[5]) / (k2[0]*k2[4])); - const auto inv2_k22 = float(1 / k2[4]); - const auto inv2_k23 = float(-k2[5] / k2[4]); + const auto inv2_k11 = float(1 / K2(0, 0)); + const auto inv2_k12 = float(-K2(0, 1) / (K2(0, 0)*K2(1, 1))); + const auto inv2_k13 = float((-K2(0, 2)*K2(1, 1) + K2(0, 1)*K2(1, 2)) / (K2(0, 0)*K2(1, 1))); + const auto inv2_k22 = float(1 / K2(1, 1)); + const auto inv2_k23 = float(-K2(1, 2) / K2(1, 1)); + const auto * const points_ = (float *) points.data; calib_points = Mat ( points.rows, 4, points.type()); auto * calib_points_ = (float *) calib_points.data; @@ -54,15 +52,13 @@ void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Ma * points is matrix of size |N| x 5, first two columns are image points [u_i, v_i] * calib_norm_pts are K^-1 [u v 1]^T / ||K^-1 [u v 1]^T|| */ -void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts) { +void Utils::calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts) { const auto * const points = (float *) pts.data; - const auto * const k = (double *) K.data; - const auto inv_k11 = float(1 / k[0]); - const auto inv_k12 = float(-k[1] / (k[0]*k[4])); - const auto inv_k13 = float((-k[2]*k[4] + k[1]*k[5]) / (k[0]*k[4])); - const auto inv_k22 = float(1 / k[4]); - const auto inv_k23 = float(-k[5] / k[4]); - + const auto inv_k11 = float(1 / K(0, 0)); + const auto inv_k12 = float(-K(0, 1) / (K(0, 0)*K(1, 1))); + const auto inv_k13 = float((-K(0, 2)*K(1, 1) + K(0, 1)*K(1, 2)) / (K(0, 0)*K(1, 1))); + const auto inv_k22 = float(1 / K(1, 1)); + const auto inv_k23 = float(-K(1, 2) / K(1, 1)); calib_norm_pts = Mat (pts.rows, 3, pts.type()); auto * calib_norm_pts_ = (float *) calib_norm_pts.data; @@ -77,10 +73,9 @@ void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &c } } -void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_norm_pts) { - const auto * const K = (double *) K_.data; - const auto k11 = (float)K[0], k12 = (float)K[1], k13 = (float)K[2], - k22 = (float)K[4], k23 = (float)K[5]; +void Utils::normalizeAndDecalibPointsPnP (const Matx33d &K_, Mat &pts, Mat &calib_norm_pts) { + const auto k11 = (float)K_(0, 0), k12 = (float)K_(0, 1), k13 = (float)K_(0, 2), + k22 = (float)K_(1, 1), k23 = (float)K_(1, 2); calib_norm_pts = Mat (pts.rows, 3, pts.type()); auto * points = (float *) pts.data; auto * calib_norm_pts_ = (float *) calib_norm_pts.data; @@ -103,20 +98,19 @@ void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_no * 0 fy ty * 0 0 1] */ -void Utils::decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal) { +void Utils::decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal) { const Mat M = P.colRange(0,3); double scale = norm(M.row(2)); scale *= scale; - Matx33d K = Matx33d::eye(); - K(1,2) = M.row(1).dot(M.row(2)) / scale; - K(0,2) = M.row(0).dot(M.row(2)) / scale; - K(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K(1,2)*K(1,2)); - K(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K(0,2)*K(0,2)); + K_ = Matx33d::eye(); + K_(1,2) = M.row(1).dot(M.row(2)) / scale; + K_(0,2) = M.row(0).dot(M.row(2)) / scale; + K_(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K_(1,2)*K_(1,2)); + K_(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K_(0,2)*K_(0,2)); if (same_focal) - K(0,0) = K(1,1) = (K(0,0) + K(1,1)) / 2; - R = K.inv() * M / sqrt(scale); + K_(0,0) = K_(1,1) = (K_(0,0) + K_(1,1)) / 2; + R = K_.inv() * M / sqrt(scale); if (determinant(M) < 0) R *= -1; t = R * M.inv() * P.col(3); - K_ = Mat(K); } Matx33d Math::getSkewSymmetric(const Vec3d &v) { diff --git a/modules/calib3d/test/test_usac.cpp b/modules/calib3d/test/test_usac.cpp index 6e6d8cecf3..8297ab1de8 100644 --- a/modules/calib3d/test/test_usac.cpp +++ b/modules/calib3d/test/test_usac.cpp @@ -452,5 +452,32 @@ TEST(usac_testUsacParams, accuracy) { checkInliersMask(TestSolver::Homogr, inl_size, usac_params.threshold, pts1, pts2, model, mask); } +TEST(usac_solvePnPRansac, regression_21105) { + std::vector gt_inliers; + const int pts_size = 100; + double inl_ratio = 0.1; + cv::Mat img_pts, obj_pts, K1, K2; + cv::RNG &rng = cv::theRNG(); + generatePoints(rng, img_pts, obj_pts, K1, K2, false /*two calib*/, + pts_size, TestSolver ::PnP, inl_ratio, 0.15 /*noise std*/, gt_inliers); + const double conf = 0.99, thr = 2., max_iters = 1.3 * log(1 - conf) / + log(1 - pow(inl_ratio, 3 /* sample size */)); + const int flag = USAC_DEFAULT; + std::vector inliers; + cv::Matx31d rvec, tvec; + CV_Assert(cv::solvePnPRansac(obj_pts, img_pts, K1, cv::noArray(), rvec, tvec, + false, (int)max_iters, (float)thr, conf, inliers, flag)); + + cv::Mat zero_column = cv::Mat::zeros(3, 1, K1.type()); + cv::hconcat(K1, zero_column, K1); + cv::Mat K1_copy = K1.colRange(0, 3); + std::vector inliers_copy; + cv::Matx31d rvec_copy, tvec_copy; + CV_Assert(cv::solvePnPRansac(obj_pts, img_pts, K1_copy, cv::noArray(), rvec_copy, tvec_copy, + false, (int)max_iters, (float)thr, conf, inliers_copy, flag)); + EXPECT_EQ(rvec, rvec_copy); + EXPECT_EQ(tvec, tvec_copy); + EXPECT_EQ(inliers, inliers_copy); +} }} // namespace