From d1b4b46dc61e3552b03bd440c39283a75d89f0a2 Mon Sep 17 00:00:00 2001 From: Manolis Lourakis <60357220+mlourakis@users.noreply.github.com> Date: Tue, 17 Jun 2025 17:11:19 +0300 Subject: [PATCH] Merge pull request #27437 from mlourakis:4.x Fixed bugs in orthogonalization; simplified column vectors copying #27437 This PR mirrors to OpenCV a bug fix addressed by commit [a03d34b](https://github.com/terzakig/sqpnp/commit/a03d34b641ebba2986cf457cd910218cc8d3cc8c) in SQPnP It also fixes bugs in the orthogonalization introduced during the porting to OpenCV and simplifies column vectors copying, eliminating double loops. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] 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 - [ ] 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/sqpnp.cpp | 63 +++++++++++++++-------------------- 1 file changed, 26 insertions(+), 37 deletions(-) diff --git a/modules/calib3d/src/sqpnp.cpp b/modules/calib3d/src/sqpnp.cpp index ef2a047d80..f03da22f1e 100644 --- a/modules/calib3d/src/sqpnp.cpp +++ b/modules/calib3d/src/sqpnp.cpp @@ -65,21 +65,6 @@ const double PoseSolver::POINT_VARIANCE_THRESHOLD = 1e-5; const double PoseSolver::SQRT3 = std::sqrt(3); const int PoseSolver::SQP_MAX_ITERATION = 15; -// No checking done here for overflow, since this is not public all call instances -// are assumed to be valid -template - void set(int row, int col, cv::Matx& dest, - const cv::Matx& source) -{ - for (int y = 0; y < snrows; y++) - { - for (int x = 0; x < sncols; x++) - { - dest(row + y, col + x) = source(y, x); - } - } -} PoseSolver::PoseSolver() : num_null_vectors_(-1), @@ -280,7 +265,7 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) // EVD equivalent of the SVD; less accurate cv::eigen(omega_, s_, u_); u_ = u_.t(); // eigenvectors were returned as rows -#endif +#endif // EVD #endif // HAVE_EIGEN @@ -601,9 +586,10 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, H(5, 4) = r(8) - dot_j5q2 * H(5, 1) - dot_j5q4 * H(5, 3); H(6, 4) = r(3) - dot_j5q3 * H(6, 2); H(7, 4) = r(4) - dot_j5q3 * H(7, 2); H(8, 4) = r(5) - dot_j5q3 * H(8, 2); - Matx q4 = H.col(4); - q4 *= (1.0 / cv::norm(q4)); - set(0, 4, H, q4); + cv::Matx q4 = H.col(4); + const double inv_norm_q4 = 1.0 / cv::norm(q4); + for (int i = 0; i < 9; ++i) + H(i, 4) = q4(i) * inv_norm_q4; K(4, 0) = 0; K(4, 1) = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1); @@ -630,9 +616,10 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, H(7, 5) = r(1) - dot_j6q3 * H(7, 2) - dot_j6q5 * H(7, 4); H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4); - Matx q5 = H.col(5); - q5 *= (1.0 / cv::norm(q5)); - set(0, 5, H, q5); + cv::Matx q5 = H.col(5); + const double inv_norm_q5 = 1.0 / cv::norm(q5); + for (int i = 0; i < 9; ++i) + H(i, 5) = q5(i) * inv_norm_q5; K(5, 0) = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); K(5, 1) = 0; K(5, 2) = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2); @@ -670,9 +657,10 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, } } - Matx v1 = Pn.col(index1); - v1 /= max_norm1; - set(0, 0, N, v1); + const cv::Matx& v1 = Pn.col(index1); + const double inv_max_norm1 = 1.0 / max_norm1; + for (int i = 0; i < 9; ++i) + N(i, 0) = v1(i) * inv_max_norm1; col_norms[index1] = -1.0; // mark to avoid use in subsequent loops for (int i = 0; i < 9; i++) @@ -690,11 +678,12 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, } } - Matx v2 = Pn.col(index2); - Matx n0 = N.col(0); - v2 -= v2.dot(n0) * n0; - v2 *= (1.0 / cv::norm(v2)); - set(0, 1, N, v2); + const cv::Matx& v2 = Pn.col(index2); + const cv::Matx& n0 = N.col(0); + cv::Matx u2 = v2 - v2.dot(n0) * n0; + const double inv_norm_u2 = 1.0 / cv::norm(u2); + for (int i = 0; i < 9; ++i) + N(i, 1) = u2(i) * inv_norm_u2; col_norms[index2] = -1.0; // mark to avoid use in subsequent loops for (int i = 0; i < 9; i++) @@ -709,17 +698,17 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, if (cos_v1_x_col + cos_v2_x_col <= min_dot1323) { index3 = i; - min_dot1323 = cos_v2_x_col + cos_v2_x_col; + min_dot1323 = cos_v1_x_col + cos_v2_x_col; } } } - Matx v3 = Pn.col(index3); - Matx n1 = N.col(1); - v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0; - v3 *= (1.0 / cv::norm(v3)); - set(0, 2, N, v3); - + const cv::Matx& v3 = Pn.col(index3); + const cv::Matx& n1 = N.col(1); + cv::Matx u3 = v3 - (v3.dot(n1) * n1) - (v3.dot(n0) * n0); + const double inv_norm_u3 = 1.0 / cv::norm(u3); + for (int i = 0; i < 9; ++i) + N(i, 2) = u3(i) * inv_norm_u3; } // if e = u*w*vt then r=u*diag([1, 1, det(u)*det(v)])*vt