From 114c23e41108a68a6dce5ec9ab8a900bccc47637 Mon Sep 17 00:00:00 2001 From: alexander-varjo <118199184+alexander-varjo@users.noreply.github.com> Date: Mon, 4 Sep 2023 17:49:45 +0300 Subject: [PATCH] Merge pull request #23607 from alexander-varjo:alexander-varjo-patch-1 Fix crash in ap3p #23607 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [ ] I agree to contribute to the project under Apache 2 License. - [ ] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/calib3d/src/ap3p.cpp | 65 ++----------------- modules/calib3d/test/test_solvepnp_ransac.cpp | 62 ++++++++++++++++++ 2 files changed, 69 insertions(+), 58 deletions(-) diff --git a/modules/calib3d/src/ap3p.cpp b/modules/calib3d/src/ap3p.cpp index 582b201b36..79da0f13a7 100644 --- a/modules/calib3d/src/ap3p.cpp +++ b/modules/calib3d/src/ap3p.cpp @@ -1,5 +1,6 @@ #include "precomp.hpp" #include "ap3p.h" +#include "polynom_solver.h" #include #include @@ -8,63 +9,10 @@ static inline double cbrt(double x) { return (double)cv::cubeRoot((float)x); }; #endif namespace { -void solveQuartic(const double *factors, double *realRoots) { - const double &a4 = factors[0]; - const double &a3 = factors[1]; - const double &a2 = factors[2]; - const double &a1 = factors[3]; - const double &a0 = factors[4]; - - double a4_2 = a4 * a4; - double a3_2 = a3 * a3; - double a4_3 = a4_2 * a4; - double a2a4 = a2 * a4; - - double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2); - double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3); - double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4)); - - double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3 - double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2 - - double t; // *=2 - std::complex w; - if (q3 >= 0) - w = -std::sqrt(static_cast >(q3 * q3 - p3 * p3 * p3)) - q3; - else - w = std::sqrt(static_cast >(q3 * q3 - p3 * p3 * p3)) - q3; - if (w.imag() == 0.0) { - w.real(std::cbrt(w.real())); - t = 2.0 * (w.real() + p3 / w.real()); - } else { - w = pow(w, 1.0 / 3); - t = 4.0 * w.real(); - } - - std::complex sqrt_2m = sqrt(static_cast >(-2 * p4 / 3 + t)); - double B_4A = -a3 / (4 * a4); - double complex1 = 4 * p4 / 3 + t; -#if defined(__clang__) && defined(__arm__) && (__clang_major__ == 3 || __clang_major__ == 4) && !defined(__ANDROID__) - // details: https://github.com/opencv/opencv/issues/11135 - // details: https://github.com/opencv/opencv/issues/11056 - std::complex complex2 = 2 * q4; - complex2 = std::complex(complex2.real() / sqrt_2m.real(), 0); -#else - std::complex complex2 = 2 * q4 / sqrt_2m; -#endif - double sqrt_2m_rh = sqrt_2m.real() / 2; - double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2; - realRoots[0] = B_4A + sqrt_2m_rh + sqrt1; - realRoots[1] = B_4A + sqrt_2m_rh - sqrt1; - double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2; - realRoots[2] = B_4A - sqrt_2m_rh + sqrt2; - realRoots[3] = B_4A - sqrt_2m_rh - sqrt2; -} - -void polishQuarticRoots(const double *coeffs, double *roots) { +void polishQuarticRoots(const double *coeffs, double *roots, int nb_roots) { const int iterations = 2; for (int i = 0; i < iterations; ++i) { - for (int j = 0; j < 4; ++j) { + for (int j = 0; j < nb_roots; ++j) { double error = (((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] + coeffs[4]; @@ -227,8 +175,9 @@ int ap3p::computePoses(const double featureVectors[3][4], 2 * (g6 * g7 - g1 * g2 - g3 * g4), g7 * g7 - g2 * g2 - g4 * g4}; double s[4]; - solveQuartic(coeffs, s); - polishQuarticRoots(coeffs, s); + int nb_roots = solve_deg4(coeffs[0], coeffs[1], coeffs[2], coeffs[3], coeffs[4], + s[0], s[1], s[2], s[3]); + polishQuarticRoots(coeffs, s, nb_roots); double temp[3]; vect_cross(k1, nl, temp); @@ -254,7 +203,7 @@ int ap3p::computePoses(const double featureVectors[3][4], double reproj_errors[4]; int nb_solutions = 0; - for (int i = 0; i < 4; ++i) { + for (int i = 0; i < nb_roots; ++i) { double ctheta1p = s[i]; if (abs(ctheta1p) > 1) continue; diff --git a/modules/calib3d/test/test_solvepnp_ransac.cpp b/modules/calib3d/test/test_solvepnp_ransac.cpp index 759b9650a8..a9ed88f0f5 100644 --- a/modules/calib3d/test/test_solvepnp_ransac.cpp +++ b/modules/calib3d/test/test_solvepnp_ransac.cpp @@ -41,6 +41,7 @@ //M*/ #include "test_precomp.hpp" +#include "opencv2/core/utils/logger.hpp" namespace opencv_test { namespace { @@ -2258,4 +2259,65 @@ TEST(Calib3d_SolvePnP, inputShape) } } +bool hasNan(const cv::Mat& mat) +{ + bool has = false; + if (mat.type() == CV_32F) + { + for(int i = 0; i < static_cast(mat.total()); i++) + has |= cvIsNaN(mat.at(i)) != 0; + } + else if (mat.type() == CV_64F) + { + for(int i = 0; i < static_cast(mat.total()); i++) + has |= cvIsNaN(mat.at(i)) != 0; + } + else + { + has = true; + CV_LOG_ERROR(NULL, "check hasNan called with unsupported type!"); + } + + return has; +} + +TEST(AP3P, ctheta1p_nan_23607) +{ + // the task is not well defined and may not converge (empty R, t) or should + // converge to some non-NaN solution + const std::array cameraPts = { + cv::Point2d{0.042784865945577621, 0.59844839572906494}, + cv::Point2d{-0.028428621590137482, 0.60354739427566528}, + cv::Point2d{0.0046037044376134872, 0.70674681663513184} + }; + const std::array modelPts = { + cv::Point3d{-0.043258000165224075, 0.020459245890378952, -0.0069921980611979961}, + cv::Point3d{-0.045648999512195587, 0.0029820732306689024, 0.0079000638797879219}, + cv::Point3d{-0.043276999145746231, -0.013622495345771313, 0.0080113131552934647} + }; + + std::vector R, t; + solveP3P(modelPts, cameraPts, Mat::eye(3, 3, CV_64F), Mat(), R, t, SOLVEPNP_AP3P); + + EXPECT_EQ(R.size(), 2ul); + EXPECT_EQ(t.size(), 2ul); + + // Try apply rvec and tvec to get model points from camera points. + Mat pts = Mat(modelPts).reshape(1, 3); + Mat expected = Mat(cameraPts).reshape(1, 3); + for (size_t i = 0; i < R.size(); ++i) { + EXPECT_TRUE(!hasNan(R[i])); + EXPECT_TRUE(!hasNan(t[i])); + + Mat transform; + cv::Rodrigues(R[i], transform); + Mat res = pts * transform.t(); + for (int j = 0; j < 3; ++j) { + res.row(j) += t[i].reshape(1, 1); + res.row(j) /= res.row(j).at(2); + } + EXPECT_LE(cvtest::norm(res.colRange(0, 2), expected, NORM_INF), 3e-16); + } +} + }} // namespace