diff --git a/modules/calib3d/src/sqpnp.cpp b/modules/calib3d/src/sqpnp.cpp index 7117e61c96c2..3e0d7ace495f 100644 --- a/modules/calib3d/src/sqpnp.cpp +++ b/modules/calib3d/src/sqpnp.cpp @@ -118,7 +118,7 @@ void PoseSolver::solve(InputArray objectPoints, InputArray imagePoints, OutputAr num_solutions_ = 0; computeOmega(_objectPoints, _imagePoints); - solveInternal(); + solveInternal(_objectPoints); int depthRot = rvecs.fixedType() ? rvecs.depth() : CV_64F; int depthTrans = tvecs.fixedType() ? tvecs.depth() : CV_64F; @@ -194,37 +194,41 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) omega_(7, 7) += sq_norm * Y2; omega_(7, 8) += sq_norm * YZ; omega_(8, 8) += sq_norm * Z2; - //Compute qa_sum + //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(1, 3) += X; qa_sum(1, 4) += Y; qa_sum(1, 5) += 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, 0) += -x * X; qa_sum(2, 1) += -x * Y; qa_sum(2, 2) += -x * Z; - qa_sum(2, 3) += -y * X; qa_sum(2, 4) += -y * Y; qa_sum(2, 5) += -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 + 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) 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) 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 upper triangle to lower triangle - for (int r = 0; r < 9; r++) - { - for (int c = 0; c < r; c++) - { - omega_(r, c) = omega_(c, r); - } - } + 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 + 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); + omega_(4, 0) = omega_(0, 4); omega_(4, 1) = omega_(1, 4); omega_(4, 2) = omega_(2, 4); omega_(4, 3) = omega_(3, 4); + omega_(5, 0) = omega_(0, 5); omega_(5, 1) = omega_(1, 5); omega_(5, 2) = omega_(2, 5); omega_(5, 3) = omega_(3, 5); omega_(5, 4) = omega_(4, 5); + omega_(6, 0) = omega_(0, 6); omega_(6, 1) = omega_(1, 6); omega_(6, 2) = omega_(2, 6); omega_(6, 3) = omega_(3, 6); omega_(6, 4) = omega_(4, 6); omega_(6, 5) = omega_(5, 6); + omega_(7, 0) = omega_(0, 7); omega_(7, 1) = omega_(1, 7); omega_(7, 2) = omega_(2, 7); omega_(7, 3) = omega_(3, 7); omega_(7, 4) = omega_(4, 7); omega_(7, 5) = omega_(5, 7); + omega_(8, 0) = omega_(0, 8); omega_(8, 1) = omega_(1, 8); omega_(8, 2) = omega_(2, 8); omega_(8, 3) = omega_(3, 8); omega_(8, 4) = omega_(4, 8); omega_(8, 5) = omega_(5, 8); cv::Matx q; q(0, 0) = n; q(0, 1) = 0; q(0, 2) = -sum_img.x; @@ -247,6 +251,11 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) cv::SVD omega_svd(omega_, cv::SVD::FULL_UV); s_ = omega_svd.w; u_ = cv::Mat(omega_svd.vt.t()); +#if 0 + // EVD equivalent of the SVD; less accurate + cv::eigen(omega_, s_, u_); + u_ = u_.t(); // eigenvectors were returned as rows +#endif CV_Assert(s_(0) >= 1e-7); @@ -257,7 +266,7 @@ void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints) point_mean_ = cv::Vec3d(sum_obj.x / n, sum_obj.y / n, sum_obj.z / n); } -void PoseSolver::solveInternal() +void PoseSolver::solveInternal(InputArray objectPoints) { double min_sq_err = std::numeric_limits::max(); int num_eigen_points = num_null_vectors_ > 0 ? num_null_vectors_ : 1; @@ -274,42 +283,39 @@ void PoseSolver::solveInternal() { solutions[0].r_hat = det3x3(e) * e; solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); + checkSolution(solutions[0], objectPoints, min_sq_err); } else { Matx r; - nearestRotationMatrix(e, r); + nearestRotationMatrixFOAM(e, r); solutions[0] = runSQP(r); solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); + checkSolution(solutions[0], objectPoints, min_sq_err); - nearestRotationMatrix(-e, r); + nearestRotationMatrixFOAM(-e, r); solutions[1] = runSQP(r); solutions[1].t = p_ * solutions[1].r_hat; - checkSolution(solutions[1], min_sq_err); + checkSolution(solutions[1], objectPoints, min_sq_err); } } - int c = 1; - - while (min_sq_err > 3 * s_[9 - num_eigen_points - c] && 9 - num_eigen_points - c > 0) + int index, c = 1; + while ((index = 9 - num_eigen_points - c) > 0 && min_sq_err > 3 * s_[index]) { - int index = 9 - num_eigen_points - c; - const cv::Matx e = u_.col(index); SQPSolution solutions[2]; Matx r; - nearestRotationMatrix(e, r); + nearestRotationMatrixFOAM(e, r); solutions[0] = runSQP(r); solutions[0].t = p_ * solutions[0].r_hat; - checkSolution(solutions[0], min_sq_err); + checkSolution(solutions[0], objectPoints, min_sq_err); - nearestRotationMatrix(-e, r); + nearestRotationMatrixFOAM(-e, r); solutions[1] = runSQP(r); solutions[1].t = p_ * solutions[1].r_hat; - checkSolution(solutions[1], min_sq_err); + checkSolution(solutions[1], objectPoints, min_sq_err); c++; } @@ -341,7 +347,7 @@ PoseSolver::SQPSolution PoseSolver::runSQP(const cv::Matx& r0) if (det_r > SQP_DET_THRESHOLD) { - nearestRotationMatrix(r, solution.r_hat); + nearestRotationMatrixFOAM(r, solution.r_hat); } else { @@ -615,12 +621,26 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, } -// faster nearest rotation computation based on FOAM (see: http://users.ics.forth.gr/~lourakis/publ/2018_iros.pdf ) +// if e = u*w*vt then r=u*diag([1, 1, det(u)*det(v)])*vt +void PoseSolver::nearestRotationMatrixSVD(const cv::Matx& e, + cv::Matx& r) +{ + cv::Matx e33 = e.reshape<3, 3>(); + cv::SVD e33_svd(e33, cv::SVD::FULL_UV); + double detuv = cv::determinant(e33_svd.u)*cv::determinant(e33_svd.vt); + cv::Matx diag = cv::Matx33d::eye(); + diag(2, 2) = detuv; + cv::Matx r33 = cv::Mat(e33_svd.u*diag*e33_svd.vt); + r = r33.reshape<9, 1>(); +} + +// 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. + * "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 * @@ -628,24 +648,32 @@ void PoseSolver::computeRowAndNullspace(const cv::Matx& r, * Institute of Computer Science, Foundation for Research & Technology - Hellas * Heraklion, Crete, Greece. */ -void PoseSolver::nearestRotationMatrix(const cv::Matx& e, +void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx& e, cv::Matx& 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); + if (fabs(det_e) < 1E-04) { // singular, handle it with SVD + PoseSolver::nearestRotationMatrixSVD(e, r); + return; + } + // e's adjoint adj_e[0] = e(4) * e(8) - e(5) * e(7); adj_e[1] = e(2) * e(7) - e(1) * e(8); adj_e[2] = e(1) * e(5) - e(2) * e(4); adj_e[3] = e(5) * e(6) - e(3) * e(8); adj_e[4] = e(0) * e(8) - e(2) * e(6); adj_e[5] = e(2) * e(3) - e(0) * e(5); 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); - // det(e), ||e||^2, ||adj(e)||^2 - 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); + // ||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]; // compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26) - for (i = 200, l = 2.0, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) { + l = 0.5*(e_sq + 3.0); // 1/2*(trace(mat(e)*mat(e)') + trace(eye(3))) + if (det_e < 0.0) l = -l; + for (i = 15, lprev = 0.0; fabs(l - lprev) > 1E-12 * fabs(lprev) && i > 0; --i) { double tmp, p, pp; tmp = (l * l - e_sq); @@ -719,9 +747,31 @@ inline bool PoseSolver::positiveDepth(const SQPSolution& solution) const return (r(6) * mean(0) + r(7) * mean(1) + r(8) * mean(2) + t(2) > 0); } -void PoseSolver::checkSolution(SQPSolution& solution, double& min_error) +inline bool PoseSolver::positiveMajorityDepths(const SQPSolution& solution, InputArray objectPoints) const +{ + const cv::Matx& r = solution.r_hat; + const cv::Matx& t = solution.t; + int npos = 0, nneg = 0; + + Mat _objectPoints = objectPoints.getMat(); + + int n = _objectPoints.cols * _objectPoints.rows; + + for (int i = 0; i < n; i++) + { + const cv::Point3d& obj_pt = _objectPoints.at(i); + if (r(6) * obj_pt.x + r(7) * obj_pt.y + r(8) * obj_pt.z + t(2) > 0) ++npos; + else ++nneg; + } + + return npos >= nneg; +} + + +void PoseSolver::checkSolution(SQPSolution& solution, InputArray objectPoints, double& min_error) { - if (positiveDepth(solution)) + bool cheirok = positiveDepth(solution) || positiveMajorityDepths(solution, objectPoints); // check the majority if the check with centroid fails + if (cheirok) { solution.sq_error = (omega_ * solution.r_hat).ddot(solution.r_hat); if (fabs(min_error - solution.sq_error) > EQUAL_SQUARED_ERRORS_DIFF) diff --git a/modules/calib3d/src/sqpnp.hpp b/modules/calib3d/src/sqpnp.hpp index 97c10e34e733..078c07e906cf 100644 --- a/modules/calib3d/src/sqpnp.hpp +++ b/modules/calib3d/src/sqpnp.hpp @@ -85,13 +85,14 @@ class PoseSolver { /* * @brief Computes the 9x9 PSD Omega matrix and supporting matrices. + * @param objectPoints The 3D points in object coordinates. */ - void solveInternal(); + void solveInternal(InputArray objectPoints); /* * @brief Produces the distance from being orthogonal for a given 3x3 matrix - * in row-major form. - * @param e The vector to test representing a 3x3 matrix in row major form. + * in row-major order. + * @param e The vector to test representing a 3x3 matrix in row-major order. * @return The distance the matrix is from being orthogonal. */ static double orthogonalityError(const cv::Matx& e); @@ -99,31 +100,49 @@ class PoseSolver { /* * @brief Processes a solution and sorts it by error. * @param solution The solution to evaluate. - * @param min_error The current minimum error. + * @param objectPoints The 3D points in object coordinates. + * @param min_error The current minimum error. */ - void checkSolution(SQPSolution& solution, double& min_error); + void checkSolution(SQPSolution& solution, InputArray objectPoints, double& min_error); /* - * @brief Computes the determinant of a matrix stored in row-major format. - * @param e Vector representing a 3x3 matrix stored in row-major format. + * @brief Computes the determinant of a matrix stored in row-major order. + * @param e Vector representing a 3x3 matrix stored in row-major order. * @return The determinant of the matrix. */ static double det3x3(const cv::Matx& e); /* - * @brief Tests the cheirality for a given solution. + * @brief Tests the cheirality on the mean object point for a given solution. * @param solution The solution to evaluate. */ inline bool positiveDepth(const SQPSolution& solution) const; /* - * @brief Determines the nearest rotation matrix to a given rotaiton matrix. - * Input and output are 9x1 vector representing a vector stored in row-major - * form. - * @param e The input 3x3 matrix stored in a vector in row-major form. - * @param r The nearest rotation matrix to the input e (again in row-major form). + * @brief Tests the cheirality on all object points for a given solution. + * @param solution The solution to evaluate. + * @param objectPoints The 3D points in object coordinates. + */ + inline bool positiveMajorityDepths(const SQPSolution& solution, InputArray objectPoints) const; + + /* + * @brief Determines the nearest rotation matrix to a given rotation matrix using SVD. + * Input and output are 9x1 vector representing a matrix stored in row-major + * order. + * @param e The input 3x3 matrix stored in a vector in row-major order. + * @param r The nearest rotation matrix to the input e (again in row-major order). + */ + static void nearestRotationMatrixSVD(const cv::Matx& e, + cv::Matx& r); + + /* + * @brief Determines the nearest rotation matrix to a given rotation matrix using the FOAM algorithm. + * Input and output are 9x1 vector representing a matrix stored in row-major + * order. + * @param e The input 3x3 matrix stored in a vector in row-major order. + * @param r The nearest rotation matrix to the input e (again in row-major order). */ - static void nearestRotationMatrix(const cv::Matx& e, + static void nearestRotationMatrixFOAM(const cv::Matx& e, cv::Matx& r); /*