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

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