Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #26219 from mlourakis:4.x

Browse Source 
SQPnP solver updates
pull/26224/head
Alexander Smorkalov 2 months ago committed by GitHub
parent 450e741f8d 086b999013
commit 658336b366
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 163 additions and 49 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 198
      modules/calib3d/src/sqpnp.cpp
  2. 14
      modules/calib3d/src/sqpnp.hpp

198
modules/calib3d/src/sqpnp.cpp
Unescape View File

@ -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,17 +737,17 @@ 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)
{
@ -655,7 +755,7 @@ void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx<double, 9, 1>& 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<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) );
}
}

14
modules/calib3d/src/sqpnp.hpp
Unescape View File

@ -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<double, 9, 1>& r, cv::Matx<double, 9, 1>& 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<double, 3, 3>& A, cv::Matx<double, 3, 3>& A1);
/*
* @brief Analytically computes the inverse of a symmetric 3x3 matrix using the
* lower triangle.

Write Preview
Loading…
Cancel
Save