diff --git a/modules/calib3d/src/ippe.cpp b/modules/calib3d/src/ippe.cpp index ec6089596c..c3d2744469 100644 --- a/modules/calib3d/src/ippe.cpp +++ b/modules/calib3d/src/ippe.cpp @@ -303,13 +303,17 @@ void PoseSolver::rot2vec(InputArray _R, OutputArray _r) Mat rvec = _r.getMat(); double trace = R.at(0, 0) + R.at(1, 1) + R.at(2, 2); - double w_norm = acos((trace - 1.0) / 2.0); + double w_norm = acos(std::min(std::max((trace - 1.0) / 2.0, -1.0), 1.0)); double eps = std::numeric_limits::epsilon(); double d = 1 / (2 * sin(w_norm)) * w_norm; if (w_norm < eps) //rotation is the identity { rvec.setTo(0); } + else if (sin(w_norm) < 1e-5) //close to 180 degrees, the axis can't be read from R - R^T + { + Rodrigues(R, rvec); + } else { double c0 = R.at(2, 1) - R.at(1, 2); diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 3cbb0d2ccf..a13468979d 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -2404,4 +2404,30 @@ TEST(AP3P, ctheta1p_nan_23607) } } +TEST(Calib3d_SolvePnP, regression_30402) +{ + // A tag seen exactly head-on: the pose has a 180 degree rotation about X. + const double L = 0.05; + std::vector objectPoints = { + Point3d(-L, L, 0), Point3d( L, L, 0), Point3d( L, -L, 0), Point3d(-L, -L, 0) + }; + Matx33d intrinsics(800, 0, 320, + 0, 800, 240, + 0, 0, 1); + Vec3d rvecGT(CV_PI, 0, 0), tvecGT(0, 0, 0.5); + + std::vector imagePoints; + projectPoints(objectPoints, rvecGT, tvecGT, intrinsics, noArray(), imagePoints); + + Mat rvec, tvec; + ASSERT_TRUE(solvePnP(objectPoints, imagePoints, intrinsics, noArray(), rvec, tvec, + false, SOLVEPNP_IPPE_SQUARE)); + + Mat R, RGT; + cv::Rodrigues(rvec, R); + cv::Rodrigues(rvecGT, RGT); + EXPECT_LE(cvtest::norm(R, RGT, NORM_INF), 1e-6); + EXPECT_LE(cvtest::norm(tvec, Mat(tvecGT), NORM_INF), 1e-6); +} + }} // namespace