1// Implementation of SQPnP as described in the paper:
2//
3// "A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem" by G. Terzakis and M. Lourakis
4// a) Paper: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460.pdf
5// b) Supplementary: https://www.ecva.net/papers/eccv_2020/papers_ECCV/papers/123460460-supp.pdf
6
7
8// This file is part of OpenCV project.
9// It is subject to the license terms in the LICENSE file found in the top-level directory
10// of this distribution and at http://opencv.org/license.html
11
12// This file is based on file issued with the following license:
13
14/*
15BSD 3-Clause License
16
17Copyright (c) 2020, George Terzakis
18All rights reserved.
19
20Redistribution and use in source and binary forms, with or without
21modification, are permitted provided that the following conditions are met:
22
231. Redistributions of source code must retain the above copyright notice, this
24 list of conditions and the following disclaimer.
25
262. Redistributions in binary form must reproduce the above copyright notice,
27 this list of conditions and the following disclaimer in the documentation
28 and/or other materials provided with the distribution.
29
303. Neither the name of the copyright holder nor the names of its
31 contributors may be used to endorse or promote products derived from
32 this software without specific prior written permission.
33
34THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
35AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
36IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
37DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
38FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
39DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
40SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
41CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
42OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
43OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
44*/
45
46#include "precomp.hpp"
47#include "sqpnp.hpp"
48
49#ifdef HAVE_EIGEN
50#include <Eigen/Dense>
51#endif
52
53#include <opencv2/calib3d.hpp>
54
55namespace cv {
56namespace sqpnp {
57
58const double PoseSolver::RANK_TOLERANCE = 1e-7;
59const double PoseSolver::SQP_SQUARED_TOLERANCE = 1e-10;
60const double PoseSolver::SQP_DET_THRESHOLD = 1.001;
61const double PoseSolver::ORTHOGONALITY_SQUARED_ERROR_THRESHOLD = 1e-8;
62const double PoseSolver::EQUAL_VECTORS_SQUARED_DIFF = 1e-10;
63const double PoseSolver::EQUAL_SQUARED_ERRORS_DIFF = 1e-6;
64const double PoseSolver::POINT_VARIANCE_THRESHOLD = 1e-5;
65const double PoseSolver::SQRT3 = std::sqrt(x: 3);
66const int PoseSolver::SQP_MAX_ITERATION = 15;
67
68
69PoseSolver::PoseSolver()
70 : num_null_vectors_(-1),
71 num_solutions_(0)
72{
73}
74
75
76void PoseSolver::solve(InputArray objectPoints, InputArray imagePoints, OutputArrayOfArrays rvecs,
77 OutputArrayOfArrays tvecs)
78{
79 // Input checking
80 int objType = objectPoints.getMat().type();
81 CV_CheckType(objType, objType == CV_32FC3 || objType == CV_64FC3,
82 "Type of objectPoints must be CV_32FC3 or CV_64FC3");
83
84 int imgType = imagePoints.getMat().type();
85 CV_CheckType(imgType, imgType == CV_32FC2 || imgType == CV_64FC2,
86 "Type of imagePoints must be CV_32FC2 or CV_64FC2");
87
88 CV_Assert(objectPoints.rows() == 1 || objectPoints.cols() == 1);
89 CV_Assert(objectPoints.rows() >= 3 || objectPoints.cols() >= 3);
90 CV_Assert(imagePoints.rows() == 1 || imagePoints.cols() == 1);
91 CV_Assert(imagePoints.rows() * imagePoints.cols() == objectPoints.rows() * objectPoints.cols());
92
93 Mat _imagePoints;
94 if (imgType == CV_32FC2)
95 {
96 imagePoints.getMat().convertTo(m: _imagePoints, CV_64F);
97 }
98 else
99 {
100 _imagePoints = imagePoints.getMat();
101 }
102
103 Mat _objectPoints;
104 if (objType == CV_32FC3)
105 {
106 objectPoints.getMat().convertTo(m: _objectPoints, CV_64F);
107 }
108 else
109 {
110 _objectPoints = objectPoints.getMat();
111 }
112
113 num_null_vectors_ = -1;
114 num_solutions_ = 0;
115
116 computeOmega(objectPoints: _objectPoints, imagePoints: _imagePoints);
117 solveInternal(objectPoints: _objectPoints);
118
119 int depthRot = rvecs.fixedType() ? rvecs.depth() : CV_64F;
120 int depthTrans = tvecs.fixedType() ? tvecs.depth() : CV_64F;
121
122 rvecs.create(rows: num_solutions_, cols: 1, CV_MAKETYPE(depthRot, rvecs.fixedType() && rvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
123 tvecs.create(rows: num_solutions_, cols: 1, CV_MAKETYPE(depthTrans, tvecs.fixedType() && tvecs.kind() == _InputArray::STD_VECTOR ? 3 : 1));
124
125 for (int i = 0; i < num_solutions_; i++)
126 {
127
128 Mat rvec;
129 Mat rotation = Mat(solutions_[i].r_hat).reshape(cn: 1, rows: 3);
130 Rodrigues(src: rotation, dst: rvec);
131
132 rvecs.getMatRef(i) = rvec;
133 tvecs.getMatRef(i) = Mat(solutions_[i].t);
134 }
135}
136
137void PoseSolver::computeOmega(InputArray objectPoints, InputArray imagePoints)
138{
139 omega_ = cv::Matx<double, 9, 9>::zeros();
140 cv::Matx<double, 3, 9> qa_sum = cv::Matx<double, 3, 9>::zeros();
141
142 cv::Point2d sum_img(0, 0);
143 cv::Point3d sum_obj(0, 0, 0);
144 double sq_norm_sum = 0;
145
146 Mat _imagePoints = imagePoints.getMat();
147 Mat _objectPoints = objectPoints.getMat();
148
149 int n = _objectPoints.cols * _objectPoints.rows;
150
151 for (int i = 0; i < n; i++)
152 {
153 const cv::Point2d& img_pt = _imagePoints.at<cv::Point2d>(i0: i);
154 const cv::Point3d& obj_pt = _objectPoints.at<cv::Point3d>(i0: i);
155
156 sum_img += img_pt;
157 sum_obj += obj_pt;
158
159 const double x = img_pt.x, y = img_pt.y;
160 const double X = obj_pt.x, Y = obj_pt.y, Z = obj_pt.z;
161 double sq_norm = x * x + y * y;
162 sq_norm_sum += sq_norm;
163
164 const double X2 = X * X,
165 XY = X * Y,
166 XZ = X * Z,
167 Y2 = Y * Y,
168 YZ = Y * Z,
169 Z2 = Z * Z;
170
171 omega_(0, 0) += X2;
172 omega_(0, 1) += XY;
173 omega_(0, 2) += XZ;
174 omega_(1, 1) += Y2;
175 omega_(1, 2) += YZ;
176 omega_(2, 2) += Z2;
177
178
179 // Populating this manually saves operations by only calculating upper triangle
180 omega_(0, 6) -= x * X2; omega_(0, 7) -= x * XY; omega_(0, 8) -= x * XZ;
181 omega_(1, 7) -= x * Y2; omega_(1, 8) -= x * YZ;
182 omega_(2, 8) -= x * Z2;
183
184 omega_(3, 6) -= y * X2; omega_(3, 7) -= y * XY; omega_(3, 8) -= y * XZ;
185 omega_(4, 7) -= y * Y2; omega_(4, 8) -= y * YZ;
186 omega_(5, 8) -= y * Z2;
187
188
189 omega_(6, 6) += sq_norm * X2; omega_(6, 7) += sq_norm * XY; omega_(6, 8) += sq_norm * XZ;
190 omega_(7, 7) += sq_norm * Y2; omega_(7, 8) += sq_norm * YZ;
191 omega_(8, 8) += sq_norm * Z2;
192
193 // Compute qa_sum. Certain pairs of elements are equal, so filling them outside the loop saves some operations
194 qa_sum(0, 0) += X; qa_sum(0, 1) += Y; qa_sum(0, 2) += Z;
195
196 qa_sum(0, 6) -= x * X; qa_sum(0, 7) -= x * Y; qa_sum(0, 8) -= x * Z;
197 qa_sum(1, 6) -= y * X; qa_sum(1, 7) -= y * Y; qa_sum(1, 8) -= y * Z;
198
199 qa_sum(2, 6) += sq_norm * X; qa_sum(2, 7) += sq_norm * Y; qa_sum(2, 8) += sq_norm * Z;
200 }
201
202 // Complete qa_sum
203 qa_sum(1, 3) = qa_sum(0, 0); qa_sum(1, 4) = qa_sum(0, 1); qa_sum(1, 5) = qa_sum(0, 2);
204 qa_sum(2, 0) = qa_sum(0, 6); qa_sum(2, 1) = qa_sum(0, 7); qa_sum(2, 2) = qa_sum(0, 8);
205 qa_sum(2, 3) = qa_sum(1, 6); qa_sum(2, 4) = qa_sum(1, 7); qa_sum(2, 5) = qa_sum(1, 8);
206
207
208 // lower triangles of omega_'s off-diagonal blocks (0:2, 6:8), (3:5, 6:8) and (6:8, 6:8)
209 omega_(1, 6) = omega_(0, 7); omega_(2, 6) = omega_(0, 8); omega_(2, 7) = omega_(1, 8);
210 omega_(4, 6) = omega_(3, 7); omega_(5, 6) = omega_(3, 8); omega_(5, 7) = omega_(4, 8);
211 omega_(7, 6) = omega_(6, 7); omega_(8, 6) = omega_(6, 8); omega_(8, 7) = omega_(7, 8);
212
213 // upper triangle of omega_'s block (3:5, 3:5)
214 omega_(3, 3) = omega_(0, 0); omega_(3, 4) = omega_(0, 1); omega_(3, 5) = omega_(0, 2);
215 omega_(4, 4) = omega_(1, 1); omega_(4, 5) = omega_(1, 2);
216 omega_(5, 5) = omega_(2, 2);
217
218 // Mirror omega_'s upper triangle to lower triangle
219 // Note that elements (7, 6), (8, 6) & (8, 7) have already been assigned above
220 omega_(1, 0) = omega_(0, 1);
221 omega_(2, 0) = omega_(0, 2); omega_(2, 1) = omega_(1, 2);
222 omega_(3, 0) = omega_(0, 3); omega_(3, 1) = omega_(1, 3); omega_(3, 2) = omega_(2, 3);
223 omega_(4, 0) = omega_(0, 4); omega_(4, 1) = omega_(1, 4); omega_(4, 2) = omega_(2, 4); omega_(4, 3) = omega_(3, 4);
224 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);
225 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);
226 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);
227 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);
228
229 cv::Matx<double, 3, 3> q;
230 q(0, 0) = n; q(0, 1) = 0; q(0, 2) = -sum_img.x;
231 q(1, 0) = 0; q(1, 1) = n; q(1, 2) = -sum_img.y;
232 q(2, 0) = -sum_img.x; q(2, 1) = -sum_img.y; q(2, 2) = sq_norm_sum;
233
234 double inv_n = 1.0 / n;
235 double detQ = n * (n * sq_norm_sum - sum_img.y * sum_img.y - sum_img.x * sum_img.x);
236 double point_coordinate_variance = detQ * inv_n * inv_n * inv_n;
237
238 CV_Assert(point_coordinate_variance >= POINT_VARIANCE_THRESHOLD);
239
240 Matx<double, 3, 3> q_inv;
241 if (!invertSPD3x3(A: q, A1&: q_inv)) analyticalInverse3x3Symm(Q: q, Qinv&: q_inv);
242
243 p_ = -q_inv * qa_sum;
244
245 omega_ += qa_sum.t() * p_;
246
247#ifdef HAVE_EIGEN
248 // Rank revealing QR nullspace computation with full pivoting.
249 // This is slightly less accurate compared to SVD but x2-x3 faster
250 Eigen::Matrix<double, 9, 9> omega_eig, tmp_eig;
251 cv::cv2eigen(omega_, omega_eig);
252 Eigen::FullPivHouseholderQR<Eigen::Matrix<double, 9, 9> > rrqr(omega_eig);
253 tmp_eig = rrqr.matrixQ();
254 cv::eigen2cv(tmp_eig, u_);
255
256 tmp_eig = rrqr.matrixQR().template triangularView<Eigen::Upper>(); // R
257 Eigen::Matrix<double, 9, 1> S_eig = tmp_eig.diagonal().array().abs();
258 cv::eigen2cv(S_eig, s_);
259#else
260 // Use OpenCV's SVD
261 cv::SVD omega_svd(omega_, cv::SVD::FULL_UV);
262 s_ = omega_svd.w;
263 u_ = cv::Mat(omega_svd.vt.t());
264#if 0
265 // EVD equivalent of the SVD; less accurate
266 cv::eigen(omega_, s_, u_);
267 u_ = u_.t(); // eigenvectors were returned as rows
268#endif // EVD
269
270#endif // HAVE_EIGEN
271
272 CV_Assert(s_(0) >= 1e-7);
273
274 while (s_(7 - num_null_vectors_) < RANK_TOLERANCE) num_null_vectors_++;
275
276 CV_Assert(++num_null_vectors_ <= 6);
277
278 point_mean_ = cv::Vec3d(sum_obj.x / n, sum_obj.y / n, sum_obj.z / n);
279}
280
281void PoseSolver::solveInternal(InputArray objectPoints)
282{
283 double min_sq_err = std::numeric_limits<double>::max();
284 int num_eigen_points = num_null_vectors_ > 0 ? num_null_vectors_ : 1;
285
286 for (int i = 9 - num_eigen_points; i < 9; i++)
287 {
288 const cv::Matx<double, 9, 1> e = SQRT3 * u_.col(j: i);
289 double orthogonality_sq_err = orthogonalityError(e);
290
291 SQPSolution solutions[2];
292
293 // If e is orthogonal, we can skip SQP
294 if (orthogonality_sq_err < ORTHOGONALITY_SQUARED_ERROR_THRESHOLD)
295 {
296 solutions[0].r_hat = det3x3(e) * e;
297 solutions[0].t = p_ * solutions[0].r_hat;
298 checkSolution(solution&: solutions[0], objectPoints, min_error&: min_sq_err);
299 }
300 else
301 {
302 Matx<double, 9, 1> r;
303 nearestRotationMatrixFOAM(e, r);
304 solutions[0] = runSQP(r0: r);
305 solutions[0].t = p_ * solutions[0].r_hat;
306 checkSolution(solution&: solutions[0], objectPoints, min_error&: min_sq_err);
307
308 nearestRotationMatrixFOAM(e: -e, r);
309 solutions[1] = runSQP(r0: r);
310 solutions[1].t = p_ * solutions[1].r_hat;
311 checkSolution(solution&: solutions[1], objectPoints, min_error&: min_sq_err);
312 }
313 }
314
315 int index, c = 1;
316 while ((index = 9 - num_eigen_points - c) > 0 && min_sq_err > 3 * s_[index])
317 {
318 const cv::Matx<double, 9, 1> e = u_.col(j: index);
319 SQPSolution solutions[2];
320
321 Matx<double, 9, 1> r;
322 nearestRotationMatrixFOAM(e, r);
323 solutions[0] = runSQP(r0: r);
324 solutions[0].t = p_ * solutions[0].r_hat;
325 checkSolution(solution&: solutions[0], objectPoints, min_error&: min_sq_err);
326
327 nearestRotationMatrixFOAM(e: -e, r);
328 solutions[1] = runSQP(r0: r);
329 solutions[1].t = p_ * solutions[1].r_hat;
330 checkSolution(solution&: solutions[1], objectPoints, min_error&: min_sq_err);
331
332 c++;
333 }
334}
335
336PoseSolver::SQPSolution PoseSolver::runSQP(const cv::Matx<double, 9, 1>& r0)
337{
338 cv::Matx<double, 9, 1> r = r0;
339
340 double delta_squared_norm = std::numeric_limits<double>::max();
341 cv::Matx<double, 9, 1> delta;
342
343 int step = 0;
344 while (delta_squared_norm > SQP_SQUARED_TOLERANCE && step++ < SQP_MAX_ITERATION)
345 {
346 solveSQPSystem(r, delta);
347 r += delta;
348 delta_squared_norm = cv::norm(M: delta, normType: cv::NORM_L2SQR);
349 }
350
351 SQPSolution solution;
352
353 double det_r = det3x3(e: r);
354 if (det_r < 0)
355 {
356 r = -r;
357 det_r = -det_r;
358 }
359
360 if (det_r > SQP_DET_THRESHOLD)
361 {
362 nearestRotationMatrixFOAM(e: r, r&: solution.r_hat);
363 }
364 else
365 {
366 solution.r_hat = r;
367 }
368
369 return solution;
370}
371
372void PoseSolver::solveSQPSystem(const cv::Matx<double, 9, 1>& r, cv::Matx<double, 9, 1>& delta)
373{
374 double sqnorm_r1 = r(0) * r(0) + r(1) * r(1) + r(2) * r(2),
375 sqnorm_r2 = r(3) * r(3) + r(4) * r(4) + r(5) * r(5),
376 sqnorm_r3 = r(6) * r(6) + r(7) * r(7) + r(8) * r(8);
377 double dot_r1r2 = r(0) * r(3) + r(1) * r(4) + r(2) * r(5),
378 dot_r1r3 = r(0) * r(6) + r(1) * r(7) + r(2) * r(8),
379 dot_r2r3 = r(3) * r(6) + r(4) * r(7) + r(5) * r(8);
380
381 cv::Matx<double, 9, 3> N;
382 cv::Matx<double, 9, 6> H;
383 cv::Matx<double, 6, 6> JH;
384
385 computeRowAndNullspace(r, H, N, K&: JH);
386
387 cv::Matx<double, 6, 1> g;
388 g(0) = 1 - sqnorm_r1; g(1) = 1 - sqnorm_r2; g(2) = 1 - sqnorm_r3; g(3) = -dot_r1r2; g(4) = -dot_r2r3; g(5) = -dot_r1r3;
389
390 cv::Matx<double, 6, 1> x;
391 x(0) = g(0) / JH(0, 0);
392 x(1) = g(1) / JH(1, 1);
393 x(2) = g(2) / JH(2, 2);
394 x(3) = (g(3) - JH(3, 0) * x(0) - JH(3, 1) * x(1)) / JH(3, 3);
395 x(4) = (g(4) - JH(4, 1) * x(1) - JH(4, 2) * x(2) - JH(4, 3) * x(3)) / JH(4, 4);
396 x(5) = (g(5) - JH(5, 0) * x(0) - JH(5, 2) * x(2) - JH(5, 3) * x(3) - JH(5, 4) * x(4)) / JH(5, 5);
397
398 delta = H * x;
399
400
401 cv::Matx<double, 3, 9> nt_omega = N.t() * omega_;
402 cv::Matx<double, 3, 3> W = nt_omega * N, W_inv;
403
404 analyticalInverse3x3Symm(Q: W, Qinv&: W_inv);
405
406 cv::Matx<double, 3, 1> y = -W_inv * nt_omega * (delta + r);
407 delta += N * y;
408}
409
410// Inverse of SPD 3x3 A via a lower triangular sqrt-free Cholesky
411// factorization A=L*D*L' (L has ones on its diagonal, D is diagonal).
412//
413// Only the lower triangular part of A is accessed.
414//
415// The function returns true if successful
416//
417// see http://euler.nmt.edu/~brian/ldlt.html
418//
419bool PoseSolver::invertSPD3x3(const cv::Matx<double, 3, 3>& A, cv::Matx<double, 3, 3>& A1)
420{
421 double L[3*3], D[3], v[2], x[3];
422
423 v[0]=D[0]=A(0, 0);
424 if(v[0]<=1E-10) return false;
425 v[1]=1.0/v[0];
426 L[3]=A(1, 0)*v[1];
427 L[6]=A(2, 0)*v[1];
428 //L[0]=1.0;
429 //L[1]=L[2]=0.0;
430
431 v[0]=L[3]*D[0];
432 v[1]=D[1]=A(1, 1)-L[3]*v[0];
433 if(v[1]<=1E-10) return false;
434 L[7]=(A(2, 1)-L[6]*v[0])/v[1];
435 //L[4]=1.0;
436 //L[5]=0.0;
437
438 v[0]=L[6]*D[0];
439 v[1]=L[7]*D[1];
440 D[2]=A(2, 2)-L[6]*v[0]-L[7]*v[1];
441 if(D[2]<=1E-10) return false;
442 //L[8]=1.0;
443
444 D[0]=1.0/D[0];
445 D[1]=1.0/D[1];
446 D[2]=1.0/D[2];
447
448 /* Forward solve Lx = e0 */
449 //x[0]=1.0;
450 x[1]=-L[3];
451 x[2]=-L[6]+L[7]*L[3];
452
453 /* Backward solve D*L'x = y */
454 A1(0, 2)=x[2]=x[2]*D[2];
455 A1(0, 1)=x[1]=x[1]*D[1]-L[7]*x[2];
456 A1(0, 0) = D[0]-L[3]*x[1]-L[6]*x[2];
457
458 /* Forward solve Lx = e1 */
459 //x[0]=0.0;
460 //x[1]=1.0;
461 x[2]=-L[7];
462
463 /* Backward solve D*L'x = y */
464 A1(1, 2)=x[2]=x[2]*D[2];
465 A1(1, 1)=x[1]= D[1]-L[7]*x[2];
466 A1(1, 0) = -L[3]*x[1]-L[6]*x[2];
467
468 /* Forward solve Lx = e2 */
469 //x[0]=0.0;
470 //x[1]=0.0;
471 //x[2]=1.0;
472
473 /* Backward solve D*L'x = y */
474 A1(2, 2)=x[2]=D[2];
475 A1(2, 1)=x[1]= -L[7]*x[2];
476 A1(2, 0) = -L[3]*x[1]-L[6]*x[2];
477
478 return true;
479}
480
481bool PoseSolver::analyticalInverse3x3Symm(const cv::Matx<double, 3, 3>& Q,
482 cv::Matx<double, 3, 3>& Qinv,
483 const double& threshold)
484{
485 // 1. Get the elements of the matrix
486 double a = Q(0, 0),
487 b = Q(1, 0), d = Q(1, 1),
488 c = Q(2, 0), e = Q(2, 1), f = Q(2, 2);
489
490 // 2. Determinant
491 double t2, t4, t7, t9, t12;
492 t2 = e * e;
493 t4 = a * d;
494 t7 = b * b;
495 t9 = b * c;
496 t12 = c * c;
497 double det = -t4 * f + a * t2 + t7 * f - 2.0 * t9 * e + t12 * d;
498
499 if (fabs(x: det) < threshold) { cv::invert(src: Q, dst: Qinv, flags: cv::DECOMP_SVD); return false; } // fall back to pseudoinverse
500
501 // 3. Inverse
502 double t15, t20, t24, t30;
503 t15 = 1.0 / det;
504 t20 = (-b * f + c * e) * t15;
505 t24 = (b * e - c * d) * t15;
506 t30 = (a * e - t9) * t15;
507 Qinv(0, 0) = (-d * f + t2) * t15;
508 Qinv(0, 1) = Qinv(1, 0) = -t20;
509 Qinv(0, 2) = Qinv(2, 0) = -t24;
510 Qinv(1, 1) = -(a * f - t12) * t15;
511 Qinv(1, 2) = Qinv(2, 1) = t30;
512 Qinv(2, 2) = -(t4 - t7) * t15;
513
514 return true;
515}
516
517void PoseSolver::computeRowAndNullspace(const cv::Matx<double, 9, 1>& r,
518 cv::Matx<double, 9, 6>& H,
519 cv::Matx<double, 9, 3>& N,
520 cv::Matx<double, 6, 6>& K,
521 const double& norm_threshold)
522{
523 H = cv::Matx<double, 9, 6>::zeros();
524
525 // 1. q1
526 double norm_r1 = sqrt(x: r(0) * r(0) + r(1) * r(1) + r(2) * r(2));
527 double inv_norm_r1 = norm_r1 > 1e-5 ? 1.0 / norm_r1 : 0.0;
528 H(0, 0) = r(0) * inv_norm_r1;
529 H(1, 0) = r(1) * inv_norm_r1;
530 H(2, 0) = r(2) * inv_norm_r1;
531 K(0, 0) = 2 * norm_r1;
532
533 // 2. q2
534 double norm_r2 = sqrt(x: r(3) * r(3) + r(4) * r(4) + r(5) * r(5));
535 double inv_norm_r2 = 1.0 / norm_r2;
536 H(3, 1) = r(3) * inv_norm_r2;
537 H(4, 1) = r(4) * inv_norm_r2;
538 H(5, 1) = r(5) * inv_norm_r2;
539 K(1, 0) = 0;
540 K(1, 1) = 2 * norm_r2;
541
542 // 3. q3 = (r3'*q2)*q2 - (r3'*q1)*q1 ; q3 = q3/norm(q3)
543 double norm_r3 = sqrt(x: r(6) * r(6) + r(7) * r(7) + r(8) * r(8));
544 double inv_norm_r3 = 1.0 / norm_r3;
545 H(6, 2) = r(6) * inv_norm_r3;
546 H(7, 2) = r(7) * inv_norm_r3;
547 H(8, 2) = r(8) * inv_norm_r3;
548 K(2, 0) = K(2, 1) = 0;
549 K(2, 2) = 2 * norm_r3;
550
551 // 4. q4
552 double dot_j4q1 = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0),
553 dot_j4q2 = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1);
554
555 H(0, 3) = r(3) - dot_j4q1 * H(0, 0);
556 H(1, 3) = r(4) - dot_j4q1 * H(1, 0);
557 H(2, 3) = r(5) - dot_j4q1 * H(2, 0);
558 H(3, 3) = r(0) - dot_j4q2 * H(3, 1);
559 H(4, 3) = r(1) - dot_j4q2 * H(4, 1);
560 H(5, 3) = r(2) - dot_j4q2 * H(5, 1);
561 double inv_norm_j4 = 1.0 / sqrt(x: H(0, 3) * H(0, 3) + H(1, 3) * H(1, 3) + H(2, 3) * H(2, 3) +
562 H(3, 3) * H(3, 3) + H(4, 3) * H(4, 3) + H(5, 3) * H(5, 3));
563
564 H(0, 3) *= inv_norm_j4;
565 H(1, 3) *= inv_norm_j4;
566 H(2, 3) *= inv_norm_j4;
567 H(3, 3) *= inv_norm_j4;
568 H(4, 3) *= inv_norm_j4;
569 H(5, 3) *= inv_norm_j4;
570
571 K(3, 0) = r(3) * H(0, 0) + r(4) * H(1, 0) + r(5) * H(2, 0);
572 K(3, 1) = r(0) * H(3, 1) + r(1) * H(4, 1) + r(2) * H(5, 1);
573 K(3, 2) = 0;
574 K(3, 3) = r(3) * H(0, 3) + r(4) * H(1, 3) + r(5) * H(2, 3) + r(0) * H(3, 3) + r(1) * H(4, 3) + r(2) * H(5, 3);
575
576 // 5. q5
577 double dot_j5q2 = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1);
578 double dot_j5q3 = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2);
579 double dot_j5q4 = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3);
580
581 H(0, 4) = -dot_j5q4 * H(0, 3);
582 H(1, 4) = -dot_j5q4 * H(1, 3);
583 H(2, 4) = -dot_j5q4 * H(2, 3);
584 H(3, 4) = r(6) - dot_j5q2 * H(3, 1) - dot_j5q4 * H(3, 3);
585 H(4, 4) = r(7) - dot_j5q2 * H(4, 1) - dot_j5q4 * H(4, 3);
586 H(5, 4) = r(8) - dot_j5q2 * H(5, 1) - dot_j5q4 * H(5, 3);
587 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);
588
589 cv::Matx<double, 9, 1> q4 = H.col(j: 4);
590 const double inv_norm_q4 = 1.0 / cv::norm(M: q4);
591 for (int i = 0; i < 9; ++i)
592 H(i, 4) = q4(i) * inv_norm_q4;
593
594 K(4, 0) = 0;
595 K(4, 1) = r(6) * H(3, 1) + r(7) * H(4, 1) + r(8) * H(5, 1);
596 K(4, 2) = r(3) * H(6, 2) + r(4) * H(7, 2) + r(5) * H(8, 2);
597 K(4, 3) = r(6) * H(3, 3) + r(7) * H(4, 3) + r(8) * H(5, 3);
598 K(4, 4) = r(6) * H(3, 4) + r(7) * H(4, 4) + r(8) * H(5, 4) + r(3) * H(6, 4) + r(4) * H(7, 4) + r(5) * H(8, 4);
599
600
601 // 4. q6
602 double dot_j6q1 = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0);
603 double dot_j6q3 = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2);
604 double dot_j6q4 = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3);
605 double dot_j6q5 = r(0) * H(6, 4) + r(1) * H(7, 4) + r(2) * H(8, 4) + r(6) * H(0, 4) + r(7) * H(1, 4) + r(8) * H(2, 4);
606
607 H(0, 5) = r(6) - dot_j6q1 * H(0, 0) - dot_j6q4 * H(0, 3) - dot_j6q5 * H(0, 4);
608 H(1, 5) = r(7) - dot_j6q1 * H(1, 0) - dot_j6q4 * H(1, 3) - dot_j6q5 * H(1, 4);
609 H(2, 5) = r(8) - dot_j6q1 * H(2, 0) - dot_j6q4 * H(2, 3) - dot_j6q5 * H(2, 4);
610
611 H(3, 5) = -dot_j6q5 * H(3, 4) - dot_j6q4 * H(3, 3);
612 H(4, 5) = -dot_j6q5 * H(4, 4) - dot_j6q4 * H(4, 3);
613 H(5, 5) = -dot_j6q5 * H(5, 4) - dot_j6q4 * H(5, 3);
614
615 H(6, 5) = r(0) - dot_j6q3 * H(6, 2) - dot_j6q5 * H(6, 4);
616 H(7, 5) = r(1) - dot_j6q3 * H(7, 2) - dot_j6q5 * H(7, 4);
617 H(8, 5) = r(2) - dot_j6q3 * H(8, 2) - dot_j6q5 * H(8, 4);
618
619 cv::Matx<double, 9, 1> q5 = H.col(j: 5);
620 const double inv_norm_q5 = 1.0 / cv::norm(M: q5);
621 for (int i = 0; i < 9; ++i)
622 H(i, 5) = q5(i) * inv_norm_q5;
623
624 K(5, 0) = r(6) * H(0, 0) + r(7) * H(1, 0) + r(8) * H(2, 0);
625 K(5, 1) = 0; K(5, 2) = r(0) * H(6, 2) + r(1) * H(7, 2) + r(2) * H(8, 2);
626 K(5, 3) = r(6) * H(0, 3) + r(7) * H(1, 3) + r(8) * H(2, 3);
627 K(5, 4) = r(6) * H(0, 4) + r(7) * H(1, 4) + r(8) * H(2, 4) + r(0) * H(6, 4) + r(1) * H(7, 4) + r(2) * H(8, 4);
628 K(5, 5) = r(6) * H(0, 5) + r(7) * H(1, 5) + r(8) * H(2, 5) + r(0) * H(6, 5) + r(1) * H(7, 5) + r(2) * H(8, 5);
629
630 // Great! Now H is an orthogonalized, sparse basis of the Jacobian row space and K is filled.
631 //
632 // Now get a projector onto the null space H:
633 const cv::Matx<double, 9, 9> Pn = cv::Matx<double, 9, 9>::eye() - (H * H.t());
634
635 // Now we need to pick 3 columns of P with non-zero norm (> 0.3) and some angle between them (> 0.3).
636 //
637 // Find the 3 columns of Pn with largest norms
638 int index1 = 0,
639 index2 = 0,
640 index3 = 0;
641 double max_norm1 = std::numeric_limits<double>::min();
642 double min_dot12 = std::numeric_limits<double>::max();
643 double min_dot1323 = std::numeric_limits<double>::max();
644
645
646 double col_norms[9];
647 for (int i = 0; i < 9; i++)
648 {
649 col_norms[i] = cv::norm(M: Pn.col(j: i));
650 if (col_norms[i] >= norm_threshold)
651 {
652 if (max_norm1 < col_norms[i])
653 {
654 max_norm1 = col_norms[i];
655 index1 = i;
656 }
657 }
658 }
659
660 const cv::Matx<double, 9, 1>& v1 = Pn.col(j: index1);
661 const double inv_max_norm1 = 1.0 / max_norm1;
662 for (int i = 0; i < 9; ++i)
663 N(i, 0) = v1(i) * inv_max_norm1;
664 col_norms[index1] = -1.0; // mark to avoid use in subsequent loops
665
666 for (int i = 0; i < 9; i++)
667 {
668 //if (i == index1) continue;
669 if (col_norms[i] >= norm_threshold)
670 {
671 double cos_v1_x_col = fabs(x: Pn.col(j: i).dot(M: v1) / col_norms[i]);
672
673 if (cos_v1_x_col <= min_dot12)
674 {
675 index2 = i;
676 min_dot12 = cos_v1_x_col;
677 }
678 }
679 }
680
681 const cv::Matx<double, 9, 1>& v2 = Pn.col(j: index2);
682 const cv::Matx<double, 9, 1>& n0 = N.col(j: 0);
683 cv::Matx<double, 9, 1> u2 = v2 - v2.dot(M: n0) * n0;
684 const double inv_norm_u2 = 1.0 / cv::norm(M: u2);
685 for (int i = 0; i < 9; ++i)
686 N(i, 1) = u2(i) * inv_norm_u2;
687 col_norms[index2] = -1.0; // mark to avoid use in subsequent loops
688
689 for (int i = 0; i < 9; i++)
690 {
691 //if (i == index2 || i == index1) continue;
692 if (col_norms[i] >= norm_threshold)
693 {
694 double inv_norm = 1.0 / col_norms[i];
695 double cos_v1_x_col = fabs(x: Pn.col(j: i).dot(M: v1) * inv_norm);
696 double cos_v2_x_col = fabs(x: Pn.col(j: i).dot(M: v2) * inv_norm);
697
698 if (cos_v1_x_col + cos_v2_x_col <= min_dot1323)
699 {
700 index3 = i;
701 min_dot1323 = cos_v1_x_col + cos_v2_x_col;
702 }
703 }
704 }
705
706 const cv::Matx<double, 9, 1>& v3 = Pn.col(j: index3);
707 const cv::Matx<double, 9, 1>& n1 = N.col(j: 1);
708 cv::Matx<double, 9, 1> u3 = v3 - (v3.dot(M: n1) * n1) - (v3.dot(M: n0) * n0);
709 const double inv_norm_u3 = 1.0 / cv::norm(M: u3);
710 for (int i = 0; i < 9; ++i)
711 N(i, 2) = u3(i) * inv_norm_u3;
712}
713
714// if e = u*w*vt then r=u*diag([1, 1, det(u)*det(v)])*vt
715void PoseSolver::nearestRotationMatrixSVD(const cv::Matx<double, 9, 1>& e,
716 cv::Matx<double, 9, 1>& r)
717{
718 cv::Matx<double, 3, 3> e33 = e.reshape<3, 3>();
719 cv::SVD e33_svd(e33, cv::SVD::FULL_UV);
720 double detuv = cv::determinant(mtx: e33_svd.u)*cv::determinant(mtx: e33_svd.vt);
721 cv::Matx<double, 3, 3> diag = cv::Matx33d::eye();
722 diag(2, 2) = detuv;
723 cv::Matx<double, 3, 3> r33 = cv::Mat(e33_svd.u*diag*e33_svd.vt);
724 r = r33.reshape<9, 1>();
725}
726
727// Faster nearest rotation computation based on FOAM. See M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016
728// and M. Lourakis, G. Terzakis: "Efficient Absolute Orientation Revisited", IROS 2018.
729/* Solve the nearest orthogonal approximation problem
730 * i.e., given e, find R minimizing ||R-e||_F
731 *
732 * The computation borrows from Markley's FOAM algorithm
733 * "Attitude Determination Using Vector Observations: A Fast Optimal Matrix Algorithm", J. Astronaut. Sci. 1993.
734 *
735 * See also M. Lourakis: "An Efficient Solution to Absolute Orientation", ICPR 2016
736 *
737 * Copyright (C) 2019 Manolis Lourakis (lourakis **at** ics forth gr)
738 * Institute of Computer Science, Foundation for Research & Technology - Hellas
739 * Heraklion, Crete, Greece.
740 */
741void PoseSolver::nearestRotationMatrixFOAM(const cv::Matx<double, 9, 1>& e,
742 cv::Matx<double, 9, 1>& r)
743{
744 int i;
745 double l, lprev, det_e, e_sq, adj_e_sq, adj_e[9];
746
747 // det(e)
748 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) );
749 if (fabs(x: det_e) < 1E-04) { // singular, handle it with SVD
750 PoseSolver::nearestRotationMatrixSVD(e, r);
751 return;
752 }
753
754 // e's adjoint
755 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);
756 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);
757 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);
758
759 // ||e||^2, ||adj(e)||^2
760 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) );
761 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] );
762
763 // compute l_max with Newton-Raphson from FOAM's characteristic polynomial, i.e. eq.(23) - (26)
764 l = 0.5*(e_sq + 3.0); // 1/2*(trace(mat(e)*mat(e)') + trace(eye(3)))
765 if (det_e < 0.0) l = -l;
766 for (i = 15, lprev = 0.0; fabs(x: l - lprev) > 1E-12 * fabs(x: lprev) && i > 0; --i) {
767 double tmp, p, pp;
768
769 tmp = (l * l - e_sq);
770 p = (tmp * tmp - 8.0 * l * det_e - 4.0 * adj_e_sq);
771 pp = 8.0 * (0.5 * tmp * l - det_e);
772
773 lprev = l;
774 l -= p / pp;
775 }
776
777 // the rotation matrix equals ((l^2 + e_sq)*e + 2*l*adj(e') - 2*e*e'*e) / (l*(l*l-e_sq) - 2*det(e)), i.e. eq.(14) using (18), (19)
778 {
779 // compute (l^2 + e_sq)*e
780 double tmp[9], e_et[9], denom;
781 const double a = l * l + e_sq;
782
783 // e_et=e*e'
784 e_et[0] = e(0) * e(0) + e(1) * e(1) + e(2) * e(2);
785 e_et[1] = e(0) * e(3) + e(1) * e(4) + e(2) * e(5);
786 e_et[2] = e(0) * e(6) + e(1) * e(7) + e(2) * e(8);
787
788 e_et[3] = e_et[1];
789 e_et[4] = e(3) * e(3) + e(4) * e(4) + e(5) * e(5);
790 e_et[5] = e(3) * e(6) + e(4) * e(7) + e(5) * e(8);
791
792 e_et[6] = e_et[2];
793 e_et[7] = e_et[5];
794 e_et[8] = e(6) * e(6) + e(7) * e(7) + e(8) * e(8);
795
796 // tmp=e_et*e
797 tmp[0] = e_et[0] * e(0) + e_et[1] * e(3) + e_et[2] * e(6);
798 tmp[1] = e_et[0] * e(1) + e_et[1] * e(4) + e_et[2] * e(7);
799 tmp[2] = e_et[0] * e(2) + e_et[1] * e(5) + e_et[2] * e(8);
800
801 tmp[3] = e_et[3] * e(0) + e_et[4] * e(3) + e_et[5] * e(6);
802 tmp[4] = e_et[3] * e(1) + e_et[4] * e(4) + e_et[5] * e(7);
803 tmp[5] = e_et[3] * e(2) + e_et[4] * e(5) + e_et[5] * e(8);
804
805 tmp[6] = e_et[6] * e(0) + e_et[7] * e(3) + e_et[8] * e(6);
806 tmp[7] = e_et[6] * e(1) + e_et[7] * e(4) + e_et[8] * e(7);
807 tmp[8] = e_et[6] * e(2) + e_et[7] * e(5) + e_et[8] * e(8);
808
809 // compute R as (a*e + 2*(l*adj(e)' - tmp))*denom; note that adj(e')=adj(e)'
810 denom = l * (l * l - e_sq) - 2.0 * det_e;
811 denom = 1.0 / denom;
812 r(0) = (a * e(0) + 2.0 * (l * adj_e[0] - tmp[0])) * denom;
813 r(1) = (a * e(1) + 2.0 * (l * adj_e[3] - tmp[1])) * denom;
814 r(2) = (a * e(2) + 2.0 * (l * adj_e[6] - tmp[2])) * denom;
815
816 r(3) = (a * e(3) + 2.0 * (l * adj_e[1] - tmp[3])) * denom;
817 r(4) = (a * e(4) + 2.0 * (l * adj_e[4] - tmp[4])) * denom;
818 r(5) = (a * e(5) + 2.0 * (l * adj_e[7] - tmp[5])) * denom;
819
820 r(6) = (a * e(6) + 2.0 * (l * adj_e[2] - tmp[6])) * denom;
821 r(7) = (a * e(7) + 2.0 * (l * adj_e[5] - tmp[7])) * denom;
822 r(8) = (a * e(8) + 2.0 * (l * adj_e[8] - tmp[8])) * denom;
823 }
824}
825
826double PoseSolver::det3x3(const cv::Matx<double, 9, 1>& e)
827{
828 return ( e(0) * e(4) * e(8) + e(1) * e(5) * e(6) + e(2) * e(3) * e(7) )
829 - ( e(6) * e(4) * e(2) + e(7) * e(5) * e(0) + e(8) * e(3) * e(1) );
830}
831
832inline bool PoseSolver::positiveDepth(const SQPSolution& solution) const
833{
834 const cv::Matx<double, 9, 1>& r = solution.r_hat;
835 const cv::Matx<double, 3, 1>& t = solution.t;
836 const cv::Vec3d& mean = point_mean_;
837 return (r(6) * mean(0) + r(7) * mean(1) + r(8) * mean(2) + t(2) > 0);
838}
839
840inline bool PoseSolver::positiveMajorityDepths(const SQPSolution& solution, InputArray objectPoints) const
841{
842 const cv::Matx<double, 9, 1>& r = solution.r_hat;
843 const cv::Matx<double, 3, 1>& t = solution.t;
844 int npos = 0, nneg = 0;
845
846 Mat _objectPoints = objectPoints.getMat();
847
848 int n = _objectPoints.cols * _objectPoints.rows;
849
850 for (int i = 0; i < n; i++)
851 {
852 const cv::Point3d& obj_pt = _objectPoints.at<cv::Point3d>(i0: i);
853 if (r(6) * obj_pt.x + r(7) * obj_pt.y + r(8) * obj_pt.z + t(2) > 0) ++npos;
854 else ++nneg;
855 }
856
857 return npos >= nneg;
858}
859
860
861void PoseSolver::checkSolution(SQPSolution& solution, InputArray objectPoints, double& min_error)
862{
863 bool cheirok = positiveDepth(solution) || positiveMajorityDepths(solution, objectPoints); // check the majority if the check with centroid fails
864 if (cheirok)
865 {
866 solution.sq_error = (omega_ * solution.r_hat).ddot(M: solution.r_hat);
867 if (fabs(x: min_error - solution.sq_error) > EQUAL_SQUARED_ERRORS_DIFF)
868 {
869 if (min_error > solution.sq_error)
870 {
871 min_error = solution.sq_error;
872 solutions_[0] = solution;
873 num_solutions_ = 1;
874 }
875 }
876 else
877 {
878 bool found = false;
879 for (int i = 0; i < num_solutions_; i++)
880 {
881 if (cv::norm(M: solutions_[i].r_hat - solution.r_hat, normType: cv::NORM_L2SQR) < EQUAL_VECTORS_SQUARED_DIFF)
882 {
883 if (solutions_[i].sq_error > solution.sq_error)
884 {
885 solutions_[i] = solution;
886 }
887 found = true;
888 break;
889 }
890 }
891
892 if (!found)
893 {
894 solutions_[num_solutions_++] = solution;
895 }
896 if (min_error > solution.sq_error) min_error = solution.sq_error;
897 }
898 }
899}
900
901double PoseSolver::orthogonalityError(const cv::Matx<double, 9, 1>& e)
902{
903 double sq_norm_e1 = e(0) * e(0) + e(1) * e(1) + e(2) * e(2);
904 double sq_norm_e2 = e(3) * e(3) + e(4) * e(4) + e(5) * e(5);
905 double sq_norm_e3 = e(6) * e(6) + e(7) * e(7) + e(8) * e(8);
906 double dot_e1e2 = e(0) * e(3) + e(1) * e(4) + e(2) * e(5);
907 double dot_e1e3 = e(0) * e(6) + e(1) * e(7) + e(2) * e(8);
908 double dot_e2e3 = e(3) * e(6) + e(4) * e(7) + e(5) * e(8);
909
910 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) +
911 2 * (dot_e1e2 * dot_e1e2 + dot_e1e3 * dot_e1e3 + dot_e2e3 * dot_e2e3) );
912}
913
914}
915}
916

source code of opencv/modules/calib3d/src/sqpnp.cpp