1 #include "precomp.hpp"
2 #include "ap3p.h"
3
4 #include <cmath>
5 #include <complex>
6 #if defined (_MSC_VER) && (_MSC_VER <= 1700)
cbrt(double x)7 static inline double cbrt(double x) { return (double)cv::cubeRoot((float)x); };
8 #endif
9
10 using namespace std;
11
12 namespace {
solveQuartic(const double * factors,double * realRoots)13 void solveQuartic(const double *factors, double *realRoots) {
14 const double &a4 = factors[0];
15 const double &a3 = factors[1];
16 const double &a2 = factors[2];
17 const double &a1 = factors[3];
18 const double &a0 = factors[4];
19
20 double a4_2 = a4 * a4;
21 double a3_2 = a3 * a3;
22 double a4_3 = a4_2 * a4;
23 double a2a4 = a2 * a4;
24
25 double p4 = (8 * a2a4 - 3 * a3_2) / (8 * a4_2);
26 double q4 = (a3_2 * a3 - 4 * a2a4 * a3 + 8 * a1 * a4_2) / (8 * a4_3);
27 double r4 = (256 * a0 * a4_3 - 3 * (a3_2 * a3_2) - 64 * a1 * a3 * a4_2 + 16 * a2a4 * a3_2) / (256 * (a4_3 * a4));
28
29 double p3 = ((p4 * p4) / 12 + r4) / 3; // /=-3
30 double q3 = (72 * r4 * p4 - 2 * p4 * p4 * p4 - 27 * q4 * q4) / 432; // /=2
31
32 double t; // *=2
33 complex<double> w;
34 if (q3 >= 0)
35 w = -sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
36 else
37 w = sqrt(static_cast<complex<double> >(q3 * q3 - p3 * p3 * p3)) - q3;
38 if (w.imag() == 0.0) {
39 w.real(cbrt(w.real()));
40 t = 2.0 * (w.real() + p3 / w.real());
41 } else {
42 w = pow(w, 1.0 / 3);
43 t = 4.0 * w.real();
44 }
45
46 complex<double> sqrt_2m = sqrt(static_cast<complex<double> >(-2 * p4 / 3 + t));
47 double B_4A = -a3 / (4 * a4);
48 double complex1 = 4 * p4 / 3 + t;
49 #if defined(__clang__) && defined(__arm__) && (__clang_major__ == 3 || __clang_major__ == 4) && !defined(__ANDROID__)
50 // details: https://github.com/opencv/opencv/issues/11135
51 // details: https://github.com/opencv/opencv/issues/11056
52 complex<double> complex2 = 2 * q4;
53 complex2 = complex<double>(complex2.real() / sqrt_2m.real(), 0);
54 #else
55 complex<double> complex2 = 2 * q4 / sqrt_2m;
56 #endif
57 double sqrt_2m_rh = sqrt_2m.real() / 2;
58 double sqrt1 = sqrt(-(complex1 + complex2)).real() / 2;
59 realRoots[0] = B_4A + sqrt_2m_rh + sqrt1;
60 realRoots[1] = B_4A + sqrt_2m_rh - sqrt1;
61 double sqrt2 = sqrt(-(complex1 - complex2)).real() / 2;
62 realRoots[2] = B_4A - sqrt_2m_rh + sqrt2;
63 realRoots[3] = B_4A - sqrt_2m_rh - sqrt2;
64 }
65
polishQuarticRoots(const double * coeffs,double * roots)66 void polishQuarticRoots(const double *coeffs, double *roots) {
67 const int iterations = 2;
68 for (int i = 0; i < iterations; ++i) {
69 for (int j = 0; j < 4; ++j) {
70 double error =
71 (((coeffs[0] * roots[j] + coeffs[1]) * roots[j] + coeffs[2]) * roots[j] + coeffs[3]) * roots[j] +
72 coeffs[4];
73 double
74 derivative =
75 ((4 * coeffs[0] * roots[j] + 3 * coeffs[1]) * roots[j] + 2 * coeffs[2]) * roots[j] + coeffs[3];
76 roots[j] -= error / derivative;
77 }
78 }
79 }
80
vect_cross(const double * a,const double * b,double * result)81 inline void vect_cross(const double *a, const double *b, double *result) {
82 result[0] = a[1] * b[2] - a[2] * b[1];
83 result[1] = -(a[0] * b[2] - a[2] * b[0]);
84 result[2] = a[0] * b[1] - a[1] * b[0];
85 }
86
vect_dot(const double * a,const double * b)87 inline double vect_dot(const double *a, const double *b) {
88 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
89 }
90
vect_norm(const double * a)91 inline double vect_norm(const double *a) {
92 return sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
93 }
94
vect_scale(const double s,const double * a,double * result)95 inline void vect_scale(const double s, const double *a, double *result) {
96 result[0] = a[0] * s;
97 result[1] = a[1] * s;
98 result[2] = a[2] * s;
99 }
100
vect_sub(const double * a,const double * b,double * result)101 inline void vect_sub(const double *a, const double *b, double *result) {
102 result[0] = a[0] - b[0];
103 result[1] = a[1] - b[1];
104 result[2] = a[2] - b[2];
105 }
106
vect_divide(const double * a,const double d,double * result)107 inline void vect_divide(const double *a, const double d, double *result) {
108 result[0] = a[0] / d;
109 result[1] = a[1] / d;
110 result[2] = a[2] / d;
111 }
112
mat_mult(const double a[3][3],const double b[3][3],double result[3][3])113 inline void mat_mult(const double a[3][3], const double b[3][3], double result[3][3]) {
114 result[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
115 result[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
116 result[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
117
118 result[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
119 result[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
120 result[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
121
122 result[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
123 result[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
124 result[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
125 }
126 }
127
128 namespace cv {
init_inverse_parameters()129 void ap3p::init_inverse_parameters() {
130 inv_fx = 1. / fx;
131 inv_fy = 1. / fy;
132 cx_fx = cx / fx;
133 cy_fy = cy / fy;
134 }
135
ap3p(cv::Mat cameraMatrix)136 ap3p::ap3p(cv::Mat cameraMatrix) {
137 if (cameraMatrix.depth() == CV_32F)
138 init_camera_parameters<float>(cameraMatrix);
139 else
140 init_camera_parameters<double>(cameraMatrix);
141 init_inverse_parameters();
142 }
143
ap3p(double _fx,double _fy,double _cx,double _cy)144 ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
145 fx = _fx;
146 fy = _fy;
147 cx = _cx;
148 cy = _cy;
149 init_inverse_parameters();
150 }
151
152 // This algorithm is from "Tong Ke, Stergios Roumeliotis, An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (Accepted by CVPR 2017)
153 // See https://arxiv.org/pdf/1701.08237.pdf
154 // featureVectors: The 3 bearing measurements (normalized) stored as column vectors
155 // worldPoints: The positions of the 3 feature points stored as column vectors
156 // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
157 // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
computePoses(const double featureVectors[3][4],const double worldPoints[3][4],double solutionsR[4][3][3],double solutionsT[4][3],bool p4p)158 int ap3p::computePoses(const double featureVectors[3][4],
159 const double worldPoints[3][4],
160 double solutionsR[4][3][3],
161 double solutionsT[4][3],
162 bool p4p) {
163
164 //world point vectors
165 double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
166 double w2[3] = {worldPoints[0][1], worldPoints[1][1], worldPoints[2][1]};
167 double w3[3] = {worldPoints[0][2], worldPoints[1][2], worldPoints[2][2]};
168 // k1
169 double u0[3];
170 vect_sub(w1, w2, u0);
171
172 double nu0 = vect_norm(u0);
173 double k1[3];
174 vect_divide(u0, nu0, k1);
175 // bi
176 double b1[3] = {featureVectors[0][0], featureVectors[1][0], featureVectors[2][0]};
177 double b2[3] = {featureVectors[0][1], featureVectors[1][1], featureVectors[2][1]};
178 double b3[3] = {featureVectors[0][2], featureVectors[1][2], featureVectors[2][2]};
179 // k3,tz
180 double k3[3];
181 vect_cross(b1, b2, k3);
182 double nk3 = vect_norm(k3);
183 vect_divide(k3, nk3, k3);
184
185 double tz[3];
186 vect_cross(b1, k3, tz);
187 // ui,vi
188 double v1[3];
189 vect_cross(b1, b3, v1);
190 double v2[3];
191 vect_cross(b2, b3, v2);
192
193 double u1[3];
194 vect_sub(w1, w3, u1);
195 // coefficients related terms
196 double u1k1 = vect_dot(u1, k1);
197 double k3b3 = vect_dot(k3, b3);
198 // f1i
199 double f11 = k3b3;
200 double f13 = vect_dot(k3, v1);
201 double f15 = -u1k1 * f11;
202 //delta
203 double nl[3];
204 vect_cross(u1, k1, nl);
205 double delta = vect_norm(nl);
206 vect_divide(nl, delta, nl);
207 f11 *= delta;
208 f13 *= delta;
209 // f2i
210 double u2k1 = u1k1 - nu0;
211 double f21 = vect_dot(tz, v2);
212 double f22 = nk3 * k3b3;
213 double f23 = vect_dot(k3, v2);
214 double f24 = u2k1 * f22;
215 double f25 = -u2k1 * f21;
216 f21 *= delta;
217 f22 *= delta;
218 f23 *= delta;
219 double g1 = f13 * f22;
220 double g2 = f13 * f25 - f15 * f23;
221 double g3 = f11 * f23 - f13 * f21;
222 double g4 = -f13 * f24;
223 double g5 = f11 * f22;
224 double g6 = f11 * f25 - f15 * f21;
225 double g7 = -f15 * f24;
226 double coeffs[5] = {g5 * g5 + g1 * g1 + g3 * g3,
227 2 * (g5 * g6 + g1 * g2 + g3 * g4),
228 g6 * g6 + 2 * g5 * g7 + g2 * g2 + g4 * g4 - g1 * g1 - g3 * g3,
229 2 * (g6 * g7 - g1 * g2 - g3 * g4),
230 g7 * g7 - g2 * g2 - g4 * g4};
231 double s[4];
232 solveQuartic(coeffs, s);
233 polishQuarticRoots(coeffs, s);
234
235 double temp[3];
236 vect_cross(k1, nl, temp);
237
238 double Ck1nl[3][3] =
239 {{k1[0], nl[0], temp[0]},
240 {k1[1], nl[1], temp[1]},
241 {k1[2], nl[2], temp[2]}};
242
243 double Cb1k3tzT[3][3] =
244 {{b1[0], b1[1], b1[2]},
245 {k3[0], k3[1], k3[2]},
246 {tz[0], tz[1], tz[2]}};
247
248 double b3p[3];
249 vect_scale((delta / k3b3), b3, b3p);
250
251 double X3 = worldPoints[0][3];
252 double Y3 = worldPoints[1][3];
253 double Z3 = worldPoints[2][3];
254 double mu3 = featureVectors[0][3];
255 double mv3 = featureVectors[1][3];
256 double reproj_errors[4];
257
258 int nb_solutions = 0;
259 for (int i = 0; i < 4; ++i) {
260 double ctheta1p = s[i];
261 if (abs(ctheta1p) > 1)
262 continue;
263 double stheta1p = sqrt(1 - ctheta1p * ctheta1p);
264 stheta1p = (k3b3 > 0) ? stheta1p : -stheta1p;
265 double ctheta3 = g1 * ctheta1p + g2;
266 double stheta3 = g3 * ctheta1p + g4;
267 double ntheta3 = stheta1p / ((g5 * ctheta1p + g6) * ctheta1p + g7);
268 ctheta3 *= ntheta3;
269 stheta3 *= ntheta3;
270
271 double C13[3][3] =
272 {{ctheta3, 0, -stheta3},
273 {stheta1p * stheta3, ctheta1p, stheta1p * ctheta3},
274 {ctheta1p * stheta3, -stheta1p, ctheta1p * ctheta3}};
275
276 double temp_matrix[3][3];
277 double R[3][3];
278 mat_mult(Ck1nl, C13, temp_matrix);
279 mat_mult(temp_matrix, Cb1k3tzT, R);
280
281 // R' * p3
282 double rp3[3] =
283 {w3[0] * R[0][0] + w3[1] * R[1][0] + w3[2] * R[2][0],
284 w3[0] * R[0][1] + w3[1] * R[1][1] + w3[2] * R[2][1],
285 w3[0] * R[0][2] + w3[1] * R[1][2] + w3[2] * R[2][2]};
286
287 double pxstheta1p[3];
288 vect_scale(stheta1p, b3p, pxstheta1p);
289
290 vect_sub(pxstheta1p, rp3, solutionsT[nb_solutions]);
291
292 solutionsR[nb_solutions][0][0] = R[0][0];
293 solutionsR[nb_solutions][1][0] = R[0][1];
294 solutionsR[nb_solutions][2][0] = R[0][2];
295 solutionsR[nb_solutions][0][1] = R[1][0];
296 solutionsR[nb_solutions][1][1] = R[1][1];
297 solutionsR[nb_solutions][2][1] = R[1][2];
298 solutionsR[nb_solutions][0][2] = R[2][0];
299 solutionsR[nb_solutions][1][2] = R[2][1];
300 solutionsR[nb_solutions][2][2] = R[2][2];
301
302 if (p4p) {
303 double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0];
304 double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1];
305 double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[nb_solutions][2];
306 double mu3p = X3p / Z3p;
307 double mv3p = Y3p / Z3p;
308 reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
309 }
310
311 nb_solutions++;
312 }
313
314 //sort the solutions
315 if (p4p) {
316 for (int i = 1; i < nb_solutions; i++) {
317 for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
318 std::swap(reproj_errors[j], reproj_errors[j-1]);
319 std::swap(solutionsR[j], solutionsR[j-1]);
320 std::swap(solutionsT[j], solutionsT[j-1]);
321 }
322 }
323 }
324
325 return nb_solutions;
326 }
327
solve(cv::Mat & R,cv::Mat & tvec,const cv::Mat & opoints,const cv::Mat & ipoints)328 bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints) {
329 CV_INSTRUMENT_REGION();
330
331 double rotation_matrix[3][3] = {}, translation[3] = {};
332 std::vector<double> points;
333 if (opoints.depth() == ipoints.depth()) {
334 if (opoints.depth() == CV_32F)
335 extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
336 else
337 extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
338 } else if (opoints.depth() == CV_32F)
339 extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
340 else
341 extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
342
343 bool result = solve(rotation_matrix, translation,
344 points[0], points[1], points[2], points[3], points[4],
345 points[5], points[6], points[7], points[8], points[9],
346 points[10], points[11], points[12], points[13],points[14],
347 points[15], points[16], points[17], points[18], points[19]);
348 cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
349 cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
350 return result;
351 }
352
solve(std::vector<cv::Mat> & Rs,std::vector<cv::Mat> & tvecs,const cv::Mat & opoints,const cv::Mat & ipoints)353 int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv::Mat &opoints, const cv::Mat &ipoints) {
354 CV_INSTRUMENT_REGION();
355
356 double rotation_matrix[4][3][3] = {}, translation[4][3] = {};
357 std::vector<double> points;
358 if (opoints.depth() == ipoints.depth()) {
359 if (opoints.depth() == CV_32F)
360 extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
361 else
362 extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
363 } else if (opoints.depth() == CV_32F)
364 extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
365 else
366 extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
367
368 const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
369 int solutions = solve(rotation_matrix, translation,
370 points[0], points[1], points[2], points[3], points[4],
371 points[5], points[6], points[7], points[8], points[9],
372 points[10], points[11], points[12], points[13], points[14],
373 points[15], points[16], points[17], points[18], points[19],
374 p4p);
375
376 for (int i = 0; i < solutions; i++) {
377 cv::Mat R, tvec;
378 cv::Mat(3, 1, CV_64F, translation[i]).copyTo(tvec);
379 cv::Mat(3, 3, CV_64F, rotation_matrix[i]).copyTo(R);
380
381 Rs.push_back(R);
382 tvecs.push_back(tvec);
383 }
384
385 return solutions;
386 }
387
388 bool
solve(double R[3][3],double t[3],double mu0,double mv0,double X0,double Y0,double Z0,double mu1,double mv1,double X1,double Y1,double Z1,double mu2,double mv2,double X2,double Y2,double Z2,double mu3,double mv3,double X3,double Y3,double Z3)389 ap3p::solve(double R[3][3], double t[3],
390 double mu0, double mv0, double X0, double Y0, double Z0,
391 double mu1, double mv1, double X1, double Y1, double Z1,
392 double mu2, double mv2, double X2, double Y2, double Z2,
393 double mu3, double mv3, double X3, double Y3, double Z3) {
394 double Rs[4][3][3] = {}, ts[4][3] = {};
395
396 const bool p4p = true;
397 int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
398 if (n == 0)
399 return false;
400
401 for (int i = 0; i < 3; i++) {
402 for (int j = 0; j < 3; j++)
403 R[i][j] = Rs[0][i][j];
404 t[i] = ts[0][i];
405 }
406
407 return true;
408 }
409
solve(double R[4][3][3],double t[4][3],double mu0,double mv0,double X0,double Y0,double Z0,double mu1,double mv1,double X1,double Y1,double Z1,double mu2,double mv2,double X2,double Y2,double Z2,double mu3,double mv3,double X3,double Y3,double Z3,bool p4p)410 int ap3p::solve(double R[4][3][3], double t[4][3],
411 double mu0, double mv0, double X0, double Y0, double Z0,
412 double mu1, double mv1, double X1, double Y1, double Z1,
413 double mu2, double mv2, double X2, double Y2, double Z2,
414 double mu3, double mv3, double X3, double Y3, double Z3,
415 bool p4p) {
416 double mk0, mk1, mk2;
417 double norm;
418
419 mu0 = inv_fx * mu0 - cx_fx;
420 mv0 = inv_fy * mv0 - cy_fy;
421 norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
422 mk0 = 1. / norm;
423 mu0 *= mk0;
424 mv0 *= mk0;
425
426 mu1 = inv_fx * mu1 - cx_fx;
427 mv1 = inv_fy * mv1 - cy_fy;
428 norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
429 mk1 = 1. / norm;
430 mu1 *= mk1;
431 mv1 *= mk1;
432
433 mu2 = inv_fx * mu2 - cx_fx;
434 mv2 = inv_fy * mv2 - cy_fy;
435 norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
436 mk2 = 1. / norm;
437 mu2 *= mk2;
438 mv2 *= mk2;
439
440 mu3 = inv_fx * mu3 - cx_fx;
441 mv3 = inv_fy * mv3 - cy_fy;
442 double mk3 = 1; //not used
443
444 double featureVectors[3][4] = {{mu0, mu1, mu2, mu3},
445 {mv0, mv1, mv2, mv3},
446 {mk0, mk1, mk2, mk3}};
447 double worldPoints[3][4] = {{X0, X1, X2, X3},
448 {Y0, Y1, Y2, Y3},
449 {Z0, Z1, Z2, Z3}};
450
451 return computePoses(featureVectors, worldPoints, R, t, p4p);
452 }
453 }
454