From 086b9990139ce96069158d363dbdf49681ae0632 Mon Sep 17 00:00:00 2001 From: Manolis Lourakis <60357220+mlourakis@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:17:22 +0300 Subject: [PATCH] SQPnP solver updates Mirror most recent changes from https://github.com/terzakig/sqpnp/pull/24 - rank revealing QR in nullspace computation - sqrt-free Cholesky (i.e., L*D*Lt) in the SQP solution - replaced divisions with multiplications by inverses - simplified checks in computeRowAndNullspace() - removed unnecessary negations - broke some dependency chains with parentheses - minor other changes --- modules/calib3d/src/sqpnp.cpp | 198 +++++++++++++++++++++++++--------- modules/calib3d/src/sqpnp.hpp | 14 +++ 2 files changed, 163 insertions(+), 49 deletions(-) diff --git a/modules/calib3d/src/sqpnp.cpp b/modules/calib3d/src/sqpnp.cpp index 3e0d7ace49..973e95d000 100644 --- a/modules/calib3d/src/sqpnp.cpp +++ b/modules/calib3d/src/sqpnp.cpp @@ -1,3 +1,10 @@ +// Implementation of SQPnP as described in the paper: +// +// "A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem" by G. Terzakis and M. Lourakis +// a) Paper: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460.pdf +// b) Supplementary: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460-supp.pdf + + // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html @@ -39,6 +46,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "precomp.hpp" #include "sqpnp.hpp" +#ifdef HAVE_EIGEN +#include +#endif + #include namespace cv { @@ -54,8 +65,8 @@ 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 +// 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, @@ -80,7 +91,7 @@ PoseSolver::PoseSolver() void PoseSolver::solve(InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs) { - //Input checking + // Input checking int objType = objectPoints.getMat().type(); CV_CheckType(objType, objType == CV_32FC3 || objType == CV_64FC3, "Type of objectPoints must be CV_32FC3 or CV_64FC3"); @@ -160,12 +171,12 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) sum_img += img_pt; sum_obj += obj_pt; - const double& x = img_pt.x, & y = img_pt.y; - const double& X = obj_pt.x, & Y = obj_pt.y, & Z = obj_pt.z; + const double x = img_pt.x, y = img_pt.y; + const double X = obj_pt.x, Y = obj_pt.y, Z = obj_pt.z; double sq_norm = x * x + y * y; sq_norm_sum += sq_norm; - double X2 = X * X, + const double X2 = X * X, XY = X * Y, XZ = X * Z, Y2 = Y * Y, @@ -180,47 +191,47 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) omega_(2, 2) += Z2; - //Populating this manually saves operations by only calculating upper triangle - omega_(0, 6) += -x * X2; omega_(0, 7) += -x * XY; omega_(0, 8) += -x * XZ; - omega_(1, 7) += -x * Y2; omega_(1, 8) += -x * YZ; - omega_(2, 8) += -x * Z2; + // Populating this manually saves operations by only calculating upper triangle + omega_(0, 6) -= x * X2; omega_(0, 7) -= x * XY; omega_(0, 8) -= x * XZ; + omega_(1, 7) -= x * Y2; omega_(1, 8) -= x * YZ; + omega_(2, 8) -= x * Z2; - omega_(3, 6) += -y * X2; omega_(3, 7) += -y * XY; omega_(3, 8) += -y * XZ; - omega_(4, 7) += -y * Y2; omega_(4, 8) += -y * YZ; - omega_(5, 8) += -y * Z2; + omega_(3, 6) -= y * X2; omega_(3, 7) -= y * XY; omega_(3, 8) -= y * XZ; + omega_(4, 7) -= y * Y2; omega_(4, 8) -= y * YZ; + omega_(5, 8) -= y * Z2; omega_(6, 6) += sq_norm * X2; omega_(6, 7) += sq_norm * XY; omega_(6, 8) += sq_norm * XZ; omega_(7, 7) += sq_norm * Y2; omega_(7, 8) += sq_norm * YZ; omega_(8, 8) += sq_norm * Z2; - //Compute qa_sum. Certain pairs of elements are equal, so filling them outside the loop saves some operations + // Compute qa_sum. Certain pairs of elements are equal, so filling them outside the loop saves some operations qa_sum(0, 0) += X; qa_sum(0, 1) += Y; qa_sum(0, 2) += Z; - qa_sum(0, 6) += -x * X; qa_sum(0, 7) += -x * Y; qa_sum(0, 8) += -x * Z; - qa_sum(1, 6) += -y * X; qa_sum(1, 7) += -y * Y; qa_sum(1, 8) += -y * Z; + qa_sum(0, 6) -= x * X; qa_sum(0, 7) -= x * Y; qa_sum(0, 8) -= x * Z; + qa_sum(1, 6) -= y * X; qa_sum(1, 7) -= y * Y; qa_sum(1, 8) -= y * Z; qa_sum(2, 6) += sq_norm * X; qa_sum(2, 7) += sq_norm * Y; qa_sum(2, 8) += sq_norm * Z; } - //Complete qa_sum + // Complete qa_sum qa_sum(1, 3) = qa_sum(0, 0); qa_sum(1, 4) = qa_sum(0, 1); qa_sum(1, 5) = qa_sum(0, 2); qa_sum(2, 0) = qa_sum(0, 6); qa_sum(2, 1) = qa_sum(0, 7); qa_sum(2, 2) = qa_sum(0, 8); qa_sum(2, 3) = qa_sum(1, 6); qa_sum(2, 4) = qa_sum(1, 7); qa_sum(2, 5) = qa_sum(1, 8); - //lower triangles of omega_'s off-diagonal blocks (0:2, 6:8), (3:5, 6:8) and (6:8, 6:8) + // lower triangles of omega_'s off-diagonal blocks (0:2, 6:8), (3:5, 6:8) and (6:8, 6:8) omega_(1, 6) = omega_(0, 7); omega_(2, 6) = omega_(0, 8); omega_(2, 7) = omega_(1, 8); omega_(4, 6) = omega_(3, 7); omega_(5, 6) = omega_(3, 8); omega_(5, 7) = omega_(4, 8); omega_(7, 6) = omega_(6, 7); omega_(8, 6) = omega_(6, 8); omega_(8, 7) = omega_(7, 8); - //upper triangle of omega_'s block (3:5, 3:5) + // upper triangle of omega_'s block (3:5, 3:5) omega_(3, 3) = omega_(0, 0); omega_(3, 4) = omega_(0, 1); omega_(3, 5) = omega_(0, 2); omega_(4, 4) = omega_(1, 1); omega_(4, 5) = omega_(1, 2); omega_(5, 5) = omega_(2, 2); - //Mirror omega_'s upper triangle to lower triangle - //Note that elements (7, 6), (8, 6) & (8, 7) have already been assigned above + // Mirror omega_'s upper triangle to lower triangle + // Note that elements (7, 6), (8, 6) & (8, 7) have already been assigned above omega_(1, 0) = omega_(0, 1); omega_(2, 0) = omega_(0, 2); omega_(2, 1) = omega_(1, 2); omega_(3, 0) = omega_(0, 3); omega_(3, 1) = omega_(1, 3); omega_(3, 2) = omega_(2, 3); @@ -242,12 +253,26 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) CV_Assert(point_coordinate_variance >= POINT_VARIANCE_THRESHOLD); Matx q_inv; - analyticalInverse3x3Symm(q, q_inv); + if (!invertSPD3x3(q, q_inv)) analyticalInverse3x3Symm(q, q_inv); p_ = -q_inv * qa_sum; omega_ += qa_sum.t() * p_; +#ifdef HAVE_EIGEN + // Rank revealing QR nullspace computation with full pivoting. + // This is slightly less accurate compared to SVD but x2-x3 faster + Eigen::Matrix omega_eig, tmp_eig; + cv::cv2eigen(omega_, omega_eig); + Eigen::FullPivHouseholderQR > rrqr(omega_eig); + tmp_eig = rrqr.matrixQ(); + cv::eigen2cv(tmp_eig, u_); + + tmp_eig = rrqr.matrixQR().template triangularView(); // R + Eigen::Matrix S_eig = tmp_eig.diagonal().array().abs(); + cv::eigen2cv(S_eig, s_); +#else + // Use OpenCV's SVD cv::SVD omega_svd(omega_, cv::SVD::FULL_UV); s_ = omega_svd.w; u_ = cv::Mat(omega_svd.vt.t()); @@ -257,6 +282,8 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) u_ = u_.t(); // eigenvectors were returned as rows #endif +#endif // HAVE_EIGEN + CV_Assert(s_(0) >= 1e-7); while (s_(7 - num_null_vectors_) < RANK_TOLERANCE) num_null_vectors_++; @@ -278,7 +305,7 @@ void PoseSolver::solveInternal(InputArray objectPoints) SQPSolution solutions[2]; - //If e is orthogonal, we can skip SQP + // If e is orthogonal, we can skip SQP if (orthogonality_sq_err < ORTHOGONALITY_SQUARED_ERROR_THRESHOLD) { solutions[0].r_hat = det3x3(e) * e; @@ -395,6 +422,76 @@ void PoseSolver::solveSQPSystem(const cv::Matx& r, cv::Matx& A, cv::Matx& A1) +{ + double L[3*3], D[3], v[2], x[3]; + + v[0]=D[0]=A(0, 0); + if(v[0]<=1E-10) return false; + v[1]=1.0/v[0]; + L[3]=A(1, 0)*v[1]; + L[6]=A(2, 0)*v[1]; + //L[0]=1.0; + //L[1]=L[2]=0.0; + + v[0]=L[3]*D[0]; + v[1]=D[1]=A(1, 1)-L[3]*v[0]; + if(v[1]<=1E-10) return false; + L[7]=(A(2, 1)-L[6]*v[0])/v[1]; + //L[4]=1.0; + //L[5]=0.0; + + v[0]=L[6]*D[0]; + v[1]=L[7]*D[1]; + D[2]=A(2, 2)-L[6]*v[0]-L[7]*v[1]; + //L[8]=1.0; + + D[0]=1.0/D[0]; + D[1]=1.0/D[1]; + D[2]=1.0/D[2]; + + /* Forward solve Lx = e0 */ + //x[0]=1.0; + x[1]=-L[3]; + x[2]=-L[6]+L[7]*L[3]; + + /* Backward solve D*L'x = y */ + A1(0, 2)=x[2]=x[2]*D[2]; + A1(0, 1)=x[1]=x[1]*D[1]-L[7]*x[2]; + A1(0, 0) = D[0]-L[3]*x[1]-L[6]*x[2]; + + /* Forward solve Lx = e1 */ + //x[0]=0.0; + //x[1]=1.0; + x[2]=-L[7]; + + /* Backward solve D*L'x = y */ + A1(1, 2)=x[2]=x[2]*D[2]; + A1(1, 1)=x[1]= D[1]-L[7]*x[2]; + A1(1, 0) = -L[3]*x[1]-L[6]*x[2]; + + /* Forward solve Lx = e2 */ + //x[0]=0.0; + //x[1]=0.0; + //x[2]=1.0; + + /* Backward solve D*L'x = y */ + A1(2, 2)=x[2]=D[2]; + A1(2, 1)=x[1]= -L[7]*x[2]; + A1(2, 0) = -L[3]*x[1]-L[6]*x[2]; + + return true; +} + bool PoseSolver::analyticalInverse3x3Symm(const cv::Matx& Q, cv::Matx& Qinv, const double& threshold) @@ -504,7 +601,7 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, 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 /= cv::norm(q4); + q4 *= (1.0 / cv::norm(q4)); set(0, 4, H, q4); K(4, 0) = 0; @@ -533,7 +630,7 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4); Matx q5 = H.col(5); - q5 /= cv::norm(q5); + q5 *= (1.0 / cv::norm(q5)); set(0, 5, H, q5); K(5, 0) = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0); @@ -575,10 +672,11 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, Matx v1 = Pn.col(index1); v1 /= max_norm1; set(0, 0, N, v1); + col_norms[index1] = -1.0; // mark to avoid use in subsequent loops for (int i = 0; i < 9; i++) { - if (i == index1) continue; + //if (i == index1) continue; if (col_norms[i] >= norm_threshold) { double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); @@ -594,16 +692,18 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, Matx v2 = Pn.col(index2); Matx n0 = N.col(0); v2 -= v2.dot(n0) * n0; - v2 /= cv::norm(v2); + v2 *= (1.0 / cv::norm(v2)); set(0, 1, N, v2); + col_norms[index2] = -1.0; // mark to avoid use in subsequent loops for (int i = 0; i < 9; i++) { - if (i == index2 || i == index1) continue; + //if (i == index2 || i == index1) continue; if (col_norms[i] >= norm_threshold) { - double cos_v1_x_col = fabs(Pn.col(i).dot(v1) / col_norms[i]); - double cos_v2_x_col = fabs(Pn.col(i).dot(v2) / col_norms[i]); + double inv_norm = 1.0 / col_norms[i]; + double cos_v1_x_col = fabs(Pn.col(i).dot(v1) * inv_norm); + double cos_v2_x_col = fabs(Pn.col(i).dot(v2) * inv_norm); if (cos_v1_x_col + cos_v2_x_col <= min_dot1323) { @@ -616,7 +716,7 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, Matx v3 = Pn.col(index3); Matx n1 = N.col(1); v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0; - v3 /= cv::norm(v3); + v3 *= (1.0 / cv::norm(v3)); set(0, 2, N, v3); } @@ -637,17 +737,17 @@ void PoseSolver::nearestRotationMatrixSVD(const cv::Matx& e, // Faster nearest rotation computation based on FOAM. See M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016 // and M. Lourakis, G. Terzakis: "Efficient Absolute Orientation Revisited", IROS 2018. /* Solve the nearest orthogonal approximation problem - * i.e., given e, find R minimizing ||R-e||_F - * - * The computation borrows from Markley's FOAM algorithm - * "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci. 1993. - * - * See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016 - * - * Copyright (C) 2019 Manolis Lourakis (lourakis **at** ics forth gr) - * Institute of Computer Science, Foundation for Research & Technology - Hellas - * Heraklion, Crete, Greece. - */ + * i.e., given e, find R minimizing ||R-e||_F + * + * The computation borrows from Markley's FOAM algorithm + * "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci. 1993. + * + * See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016 + * + * Copyright (C) 2019 Manolis Lourakis (lourakis **at** ics forth gr) + * Institute of Computer Science, Foundation for Research & Technology - Hellas + * Heraklion, Crete, Greece. + */ void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx& e, cv::Matx& r) { @@ -655,7 +755,7 @@ void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx& e, double l, lprev, det_e, e_sq, adj_e_sq, adj_e[9]; // det(e) - det_e = e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) + e(2) * e(3) * e(7) + e(1) * e(6) * e(5) - e(2) * e(6) * e(4); + det_e = ( e(0) * e(4) * e(8) - e(0) * e(5) * e(7) - e(1) * e(3) * e(8) ) + ( e(2) * e(3) * e(7) + e(1) * e(6) * e(5) - e(2) * e(6) * e(4) ); if (fabs(det_e) < 1E-04) { // singular, handle it with SVD PoseSolver::nearestRotationMatrixSVD(e, r); return; @@ -667,8 +767,8 @@ void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx& e, adj_e[6] = e(3) * e(7) - e(4) * e(6); adj_e[7] = e(1) * e(6) - e(0) * e(7); adj_e[8] = e(0) * e(4) - e(1) * e(3); // ||e||^2, ||adj(e)||^2 - e_sq = e(0) * e(0) + e(1) * e(1) + e(2) * e(2) + e(3) * e(3) + e(4) * e(4) + e(5) * e(5) + e(6) * e(6) + e(7) * e(7) + e(8) * e(8); - adj_e_sq = adj_e[0] * adj_e[0] + adj_e[1] * adj_e[1] + adj_e[2] * adj_e[2] + adj_e[3] * adj_e[3] + adj_e[4] * adj_e[4] + adj_e[5] * adj_e[5] + adj_e[6] * adj_e[6] + adj_e[7] * adj_e[7] + adj_e[8] * adj_e[8]; + e_sq = ( e(0) * e(0) + e(1) * e(1) + e(2) * e(2) ) + ( e(3) * e(3) + e(4) * e(4) + e(5) * e(5) ) + ( e(6) * e(6) + e(7) * e(7) + e(8) * e(8) ); + adj_e_sq = ( adj_e[0] * adj_e[0] + adj_e[1] * adj_e[1] + adj_e[2] * adj_e[2] ) + ( adj_e[3] * adj_e[3] + adj_e[4] * adj_e[4] + adj_e[5] * adj_e[5] ) + ( adj_e[6] * adj_e[6] + adj_e[7] * adj_e[7] + adj_e[8] * adj_e[8] ); // compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26) l = 0.5*(e_sq + 3.0); // 1/2*(trace(mat(e)*mat(e)') + trace(eye(3))) @@ -735,8 +835,8 @@ void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx& e, double PoseSolver::det3x3(const cv::Matx& e) { - return e(0) * e(4) * e(8) + e(1) * e(5) * e(6) + e(2) * e(3) * e(7) - - e(6) * e(4) * e(2) - e(7) * e(5) * e(0) - e(8) * e(3) * e(1); + return ( e(0) * e(4) * e(8) + e(1) * e(5) * e(6) + e(2) * e(3) * e(7) ) + - ( e(6) * e(4) * e(2) + e(7) * e(5) * e(0) + e(8) * e(3) * e(1) ); } inline bool PoseSolver::positiveDepth(const SQPSolution& solution) const @@ -817,8 +917,8 @@ double PoseSolver::orthogonalityError(const cv::Matx& e) double dot_e1e3 = e(0) * e(6) + e(1) * e(7) + e(2) * e(8); double dot_e2e3 = e(3) * e(6) + e(4) * e(7) + e(5) * e(8); - return (sq_norm_e1 - 1) * (sq_norm_e1 - 1) + (sq_norm_e2 - 1) * (sq_norm_e2 - 1) + (sq_norm_e3 - 1) * (sq_norm_e3 - 1) + - 2 * (dot_e1e2 * dot_e1e2 + dot_e1e3 * dot_e1e3 + dot_e2e3 * dot_e2e3); + return ( (sq_norm_e1 - 1) * (sq_norm_e1 - 1) + (sq_norm_e2 - 1) * (sq_norm_e2 - 1) ) + ( (sq_norm_e3 - 1) * (sq_norm_e3 - 1) + + 2 * (dot_e1e2 * dot_e1e2 + dot_e1e3 * dot_e1e3 + dot_e2e3 * dot_e2e3) ); } } diff --git a/modules/calib3d/src/sqpnp.hpp b/modules/calib3d/src/sqpnp.hpp index 078c07e906..ba44b8e867 100644 --- a/modules/calib3d/src/sqpnp.hpp +++ b/modules/calib3d/src/sqpnp.hpp @@ -1,3 +1,10 @@ +// Implementation of SQPnP as described in the paper: +// +// "A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem" by G. Terzakis and M. Lourakis +// a) Paper: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460.pdf +// b) Supplementary: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460-supp.pdf + + // This file is part of OpenCV project. // It is subject to the license terms in the LICENSE file found in the top-level directory // of this distribution and at http://opencv.org/license.html @@ -158,6 +165,13 @@ private: */ void solveSQPSystem(const cv::Matx& r, cv::Matx& delta); + /* + * @brief Inverse of SPD 3x3 A via lower triangular sqrt-free Cholesky: A = L*D*L' + * @param A The input matrix + * @param A1 The inverse + */ + static bool invertSPD3x3(const cv::Matx& A, cv::Matx& A1); + /* * @brief Analytically computes the inverse of a symmetric 3x3 matrix using the * lower triangle.