1 #include <cstring>
2 #include <cmath>
3 #include <iostream>
4
5 #include "polynom_solver.h"
6 #include "p3p.h"
7
init_inverse_parameters()8 void p3p::init_inverse_parameters()
9 {
10 inv_fx = 1. / fx;
11 inv_fy = 1. / fy;
12 cx_fx = cx / fx;
13 cy_fy = cy / fy;
14 }
15
p3p(cv::Mat cameraMatrix)16 p3p::p3p(cv::Mat cameraMatrix)
17 {
18 if (cameraMatrix.depth() == CV_32F)
19 init_camera_parameters<float>(cameraMatrix);
20 else
21 init_camera_parameters<double>(cameraMatrix);
22 init_inverse_parameters();
23 }
24
p3p(double _fx,double _fy,double _cx,double _cy)25 p3p::p3p(double _fx, double _fy, double _cx, double _cy)
26 {
27 fx = _fx;
28 fy = _fy;
29 cx = _cx;
30 cy = _cy;
31 init_inverse_parameters();
32 }
33
solve(cv::Mat & R,cv::Mat & tvec,const cv::Mat & opoints,const cv::Mat & ipoints)34 bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
35 {
36 double rotation_matrix[3][3], translation[3];
37 std::vector<double> points;
38 if (opoints.depth() == ipoints.depth())
39 {
40 if (opoints.depth() == CV_32F)
41 extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points);
42 else
43 extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points);
44 }
45 else if (opoints.depth() == CV_32F)
46 extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points);
47 else
48 extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points);
49
50 bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
51 points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14],
52 points[15], points[16], points[17], points[18], points[19]);
53 cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
54 cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
55 return result;
56 }
57
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)58 bool p3p::solve(double R[3][3], double t[3],
59 double mu0, double mv0, double X0, double Y0, double Z0,
60 double mu1, double mv1, double X1, double Y1, double Z1,
61 double mu2, double mv2, double X2, double Y2, double Z2,
62 double mu3, double mv3, double X3, double Y3, double Z3)
63 {
64 double Rs[4][3][3], ts[4][3];
65
66 int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
67
68 if (n == 0)
69 return false;
70
71 int ns = 0;
72 double min_reproj = 0;
73 for(int i = 0; i < n; i++) {
74 double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
75 double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
76 double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
77 double mu3p = cx + fx * X3p / Z3p;
78 double mv3p = cy + fy * Y3p / Z3p;
79 double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
80 if (i == 0 || min_reproj > reproj) {
81 ns = i;
82 min_reproj = reproj;
83 }
84 }
85
86 for(int i = 0; i < 3; i++) {
87 for(int j = 0; j < 3; j++)
88 R[i][j] = Rs[ns][i][j];
89 t[i] = ts[ns][i];
90 }
91
92 return true;
93 }
94
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)95 int p3p::solve(double R[4][3][3], double t[4][3],
96 double mu0, double mv0, double X0, double Y0, double Z0,
97 double mu1, double mv1, double X1, double Y1, double Z1,
98 double mu2, double mv2, double X2, double Y2, double Z2)
99 {
100 double mk0, mk1, mk2;
101 double norm;
102
103 mu0 = inv_fx * mu0 - cx_fx;
104 mv0 = inv_fy * mv0 - cy_fy;
105 norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
106 mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0;
107
108 mu1 = inv_fx * mu1 - cx_fx;
109 mv1 = inv_fy * mv1 - cy_fy;
110 norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
111 mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1;
112
113 mu2 = inv_fx * mu2 - cx_fx;
114 mv2 = inv_fy * mv2 - cy_fy;
115 norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
116 mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2;
117
118 double distances[3];
119 distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) );
120 distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) );
121 distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) );
122
123 // Calculate angles
124 double cosines[3];
125 cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
126 cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
127 cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
128
129 double lengths[4][3];
130 int n = solve_for_lengths(lengths, distances, cosines);
131
132 int nb_solutions = 0;
133 for(int i = 0; i < n; i++) {
134 double M_orig[3][3];
135
136 M_orig[0][0] = lengths[i][0] * mu0;
137 M_orig[0][1] = lengths[i][0] * mv0;
138 M_orig[0][2] = lengths[i][0] * mk0;
139
140 M_orig[1][0] = lengths[i][1] * mu1;
141 M_orig[1][1] = lengths[i][1] * mv1;
142 M_orig[1][2] = lengths[i][1] * mk1;
143
144 M_orig[2][0] = lengths[i][2] * mu2;
145 M_orig[2][1] = lengths[i][2] * mv2;
146 M_orig[2][2] = lengths[i][2] * mk2;
147
148 if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions]))
149 continue;
150
151 nb_solutions++;
152 }
153
154 return nb_solutions;
155 }
156
157 /// Given 3D distances between three points and cosines of 3 angles at the apex, calculates
158 /// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C).
159 /// Returned distances are for |PA|, |PB|, |PC| respectively.
160 /// Only the solution to the main branch.
161 /// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem"
162 /// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003
163 /// \param lengths3D Lengths of line segments up to four solutions.
164 /// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|.
165 /// \param cosines Cosine of the angles /_BPC, /_APC, /_APB.
166 /// \returns Number of solutions.
167 /// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED
168
solve_for_lengths(double lengths[4][3],double distances[3],double cosines[3])169 int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
170 {
171 double p = cosines[0] * 2;
172 double q = cosines[1] * 2;
173 double r = cosines[2] * 2;
174
175 double inv_d22 = 1. / (distances[2] * distances[2]);
176 double a = inv_d22 * (distances[0] * distances[0]);
177 double b = inv_d22 * (distances[1] * distances[1]);
178
179 double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
180 double pr = p * r, pqr = q * pr;
181
182 // Check reality condition (the four points should not be coplanar)
183 if (p2 + q2 + r2 - pqr - 1 == 0)
184 return 0;
185
186 double ab = a * b, a_2 = 2*a;
187
188 double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2;
189
190 // Check reality condition
191 if (A == 0) return 0;
192
193 double a_4 = 4*a;
194
195 double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab);
196 double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2;
197 double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2);
198 double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2;
199
200 double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr);
201 double b0 = b * temp * temp;
202 // Check reality condition
203 if (b0 == 0)
204 return 0;
205
206 double real_roots[4];
207 int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]);
208
209 if (n == 0)
210 return 0;
211
212 int nb_solutions = 0;
213 double r3 = r2*r, pr2 = p*r2, r3q = r3 * q;
214 double inv_b0 = 1. / b0;
215
216 // For each solution of x
217 for(int i = 0; i < n; i++) {
218 double x = real_roots[i];
219
220 // Check reality condition
221 if (x <= 0)
222 continue;
223
224 double x2 = x*x;
225
226 double b1 =
227 ((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) *
228 (((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x +
229
230 (r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 +
231
232 (r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x +
233
234 2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) +
235 p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1)));
236
237 // Check reality condition
238 if (b1 <= 0)
239 continue;
240
241 double y = inv_b0 * b1;
242 double v = x2 + y*y - x*y*r;
243
244 if (v <= 0)
245 continue;
246
247 double Z = distances[2] / sqrt(v);
248 double X = x * Z;
249 double Y = y * Z;
250
251 lengths[nb_solutions][0] = X;
252 lengths[nb_solutions][1] = Y;
253 lengths[nb_solutions][2] = Z;
254
255 nb_solutions++;
256 }
257
258 return nb_solutions;
259 }
260
align(double M_end[3][3],double X0,double Y0,double Z0,double X1,double Y1,double Z1,double X2,double Y2,double Z2,double R[3][3],double T[3])261 bool p3p::align(double M_end[3][3],
262 double X0, double Y0, double Z0,
263 double X1, double Y1, double Z1,
264 double X2, double Y2, double Z2,
265 double R[3][3], double T[3])
266 {
267 // Centroids:
268 double C_start[3], C_end[3];
269 for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
270 C_start[0] = (X0 + X1 + X2) / 3;
271 C_start[1] = (Y0 + Y1 + Y2) / 3;
272 C_start[2] = (Z0 + Z1 + Z2) / 3;
273
274 // Covariance matrix s:
275 double s[3 * 3];
276 for(int j = 0; j < 3; j++) {
277 s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0];
278 s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1];
279 s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2];
280 }
281
282 double Qs[16], evs[4], U[16];
283
284 Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
285 Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
286 Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
287 Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
288
289 Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
290 Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
291 Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
292 Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
293 Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
294 Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
295
296 jacobi_4x4(Qs, evs, U);
297
298 // Looking for the largest eigen value:
299 int i_ev = 0;
300 double ev_max = evs[i_ev];
301 for(int i = 1; i < 4; i++)
302 if (evs[i] > ev_max)
303 ev_max = evs[i_ev = i];
304
305 // Quaternion:
306 double q[4];
307 for(int i = 0; i < 4; i++)
308 q[i] = U[i * 4 + i_ev];
309
310 double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3];
311 double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
312 double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
313 double q2_3 = q[2] * q[3];
314
315 R[0][0] = q02 + q12 - q22 - q32;
316 R[0][1] = 2. * (q1_2 - q0_3);
317 R[0][2] = 2. * (q1_3 + q0_2);
318
319 R[1][0] = 2. * (q1_2 + q0_3);
320 R[1][1] = q02 + q22 - q12 - q32;
321 R[1][2] = 2. * (q2_3 - q0_1);
322
323 R[2][0] = 2. * (q1_3 - q0_2);
324 R[2][1] = 2. * (q2_3 + q0_1);
325 R[2][2] = q02 + q32 - q12 - q22;
326
327 for(int i = 0; i < 3; i++)
328 T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]);
329
330 return true;
331 }
332
jacobi_4x4(double * A,double * D,double * U)333 bool p3p::jacobi_4x4(double * A, double * D, double * U)
334 {
335 double B[4], Z[4];
336 double Id[16] = {1., 0., 0., 0.,
337 0., 1., 0., 0.,
338 0., 0., 1., 0.,
339 0., 0., 0., 1.};
340
341 memcpy(U, Id, 16 * sizeof(double));
342
343 B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15];
344 memcpy(D, B, 4 * sizeof(double));
345 memset(Z, 0, 4 * sizeof(double));
346
347 for(int iter = 0; iter < 50; iter++) {
348 double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]);
349
350 if (sum == 0.0)
351 return true;
352
353 double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
354 for(int i = 0; i < 3; i++) {
355 double * pAij = A + 5 * i + 1;
356 for(int j = i + 1 ; j < 4; j++) {
357 double Aij = *pAij;
358 double eps_machine = 100.0 * fabs(Aij);
359
360 if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) )
361 *pAij = 0.0;
362 else if (fabs(Aij) > tresh) {
363 double hh = D[j] - D[i], t;
364 if (fabs(hh) + eps_machine == fabs(hh))
365 t = Aij / hh;
366 else {
367 double theta = 0.5 * hh / Aij;
368 t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
369 if (theta < 0.0) t = -t;
370 }
371
372 hh = t * Aij;
373 Z[i] -= hh;
374 Z[j] += hh;
375 D[i] -= hh;
376 D[j] += hh;
377 *pAij = 0.0;
378
379 double c = 1.0 / sqrt(1 + t * t);
380 double s = t * c;
381 double tau = s / (1.0 + c);
382 for(int k = 0; k <= i - 1; k++) {
383 double g = A[k * 4 + i], h = A[k * 4 + j];
384 A[k * 4 + i] = g - s * (h + g * tau);
385 A[k * 4 + j] = h + s * (g - h * tau);
386 }
387 for(int k = i + 1; k <= j - 1; k++) {
388 double g = A[i * 4 + k], h = A[k * 4 + j];
389 A[i * 4 + k] = g - s * (h + g * tau);
390 A[k * 4 + j] = h + s * (g - h * tau);
391 }
392 for(int k = j + 1; k < 4; k++) {
393 double g = A[i * 4 + k], h = A[j * 4 + k];
394 A[i * 4 + k] = g - s * (h + g * tau);
395 A[j * 4 + k] = h + s * (g - h * tau);
396 }
397 for(int k = 0; k < 4; k++) {
398 double g = U[k * 4 + i], h = U[k * 4 + j];
399 U[k * 4 + i] = g - s * (h + g * tau);
400 U[k * 4 + j] = h + s * (g - h * tau);
401 }
402 }
403 pAij++;
404 }
405 }
406
407 for(int i = 0; i < 4; i++) B[i] += Z[i];
408 memcpy(D, B, 4 * sizeof(double));
409 memset(Z, 0, 4 * sizeof(double));
410 }
411
412 return false;
413 }
414