SQPnP solver updates

Mirror most recent changes from terzakig/sqpnp#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

Author: mlourakis

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 <Eigen/Dense>
+#endif
+
 #include <opencv2/calib3d.hpp>
 
 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 <typename tp, int snrows, int sncols,
     int dnrows, int dncols>
     void set(int row, int col, cv::Matx<tp, dnrows, dncols>& 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<double, 3, 3> 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<double, 9, 9> omega_eig, tmp_eig;
+    cv::cv2eigen(omega_, omega_eig);
+    Eigen::FullPivHouseholderQR<Eigen::Matrix<double, 9, 9> > rrqr(omega_eig);
+    tmp_eig = rrqr.matrixQ();
+    cv::eigen2cv(tmp_eig, u_);
+
+    tmp_eig = rrqr.matrixQR().template triangularView<Eigen::Upper>(); // R
+    Eigen::Matrix<double, 9, 1> 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<double, 9, 1>& r, cv::Matx<double
     delta += N * y;
 }
 
+// Inverse of SPD 3x3 A via a lower triangular sqrt-free Cholesky
+// factorization A=L*D*L' (L has ones on its diagonal, D is diagonal).
+//
+// Only the lower triangular part of A is accessed.
+//
+// The function returns true if successful
+//
+// see http://euler.nmt.edu/~brian/ldlt.html
+//
+bool PoseSolver::invertSPD3x3(const cv::Matx<double, 3, 3>& A, cv::Matx<double, 3, 3>& 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<double, 3, 3>& Q,
     cv::Matx<double, 3, 3>& Qinv,
     const double& threshold)
@@ -504,7 +601,7 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx<double, 9, 1>& 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<double, 9, 1> q4 = H.col(4);
-    q4 /= cv::norm(q4);
+    q4 *= (1.0 / cv::norm(q4));
     set<double, 9, 1, 9, 6>(0, 4, H, q4);
 
     K(4, 0) = 0;
@@ -533,7 +630,7 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx<double, 9, 1>& r,
     H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4);
 
     Matx<double, 9, 1> q5 = H.col(5);
-    q5 /= cv::norm(q5);
+    q5 *= (1.0 / cv::norm(q5));
     set<double, 9, 1, 9, 6>(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<double, 9, 1>& r,
     Matx<double, 9, 1> v1 = Pn.col(index1);
     v1 /= max_norm1;
     set<double, 9, 1, 9, 3>(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<double, 9, 1>& r,
     Matx<double, 9, 1> v2 = Pn.col(index2);
     Matx<double, 9, 1> n0 = N.col(0);
     v2 -= v2.dot(n0) * n0;
-    v2 /= cv::norm(v2);
+    v2 *= (1.0 / cv::norm(v2));
     set<double, 9, 1, 9, 3>(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<double, 9, 1>& r,
     Matx<double, 9, 1> v3 = Pn.col(index3);
     Matx<double, 9, 1> n1 = N.col(1);
     v3 -= (v3.dot(n1)) * n1 - (v3.dot(n0)) * n0;
-    v3 /= cv::norm(v3);
+    v3 *= (1.0 / cv::norm(v3));
     set<double, 9, 1, 9, 3>(0, 2, N, v3);
 
 }
@@ -637,25 +737,25 @@ void PoseSolver::nearestRotationMatrixSVD(const cv::Matx<double, 9, 1>& 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<double, 9, 1>& e,
     cv::Matx<double, 9, 1>& r)
 {
     int i;
     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<double, 9, 1>& 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<double, 9, 1>& e,
 
 double PoseSolver::det3x3(const cv::Matx<double, 9, 1>& 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<double, 9, 1>& 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) );
 }
 
 }