MRPT  1.9.9
p3p.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <cmath>
11 #include <cstring>
12 #include <iostream>
13 #include "vision-precomp.h" // Precompiled headers
14 
15 #include "p3p.h"
16 #include "polynom_solver.h"
17 
19 {
20  inv_fx = 1. / fx;
21  inv_fy = 1. / fy;
22  cx_fx = cx / fx;
23  cy_fy = cy / fy;
24 }
25 
26 mrpt::vision::pnp::p3p::p3p(Eigen::MatrixXd cam_intrinsic)
27 {
28  fx = cam_intrinsic(0, 0);
29  fy = cam_intrinsic(1, 1);
30  cx = cam_intrinsic(0, 2);
31  cy = cam_intrinsic(1, 2);
32  init_inverse_parameters();
33 }
34 
35 mrpt::vision::pnp::p3p::p3p(double _fx, double _fy, double _cx, double _cy)
36 {
37  fx = _fx;
38  fy = _fy;
39  cx = _cx;
40  cy = _cy;
41  init_inverse_parameters();
42 }
43 #if MRPT_HAS_OPENCV
44 
45 mrpt::vision::pnp::p3p::p3p(cv::Mat cameraMatrix)
46 {
47  if (cameraMatrix.depth() == CV_32F)
48  init_camera_parameters<float>(cameraMatrix);
49  else
50  init_camera_parameters<double>(cameraMatrix);
51  init_inverse_parameters();
52 }
53 
55  cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints)
56 {
57  double rotation_matrix[3][3], translation[3];
58  std::vector<double> points;
59  if (opoints.depth() == ipoints.depth())
60  {
61  if (opoints.depth() == CV_32F)
62  extract_points<cv::Point3f, cv::Point2f>(opoints, ipoints, points);
63  else
64  extract_points<cv::Point3d, cv::Point2d>(opoints, ipoints, points);
65  }
66  else if (opoints.depth() == CV_32F)
67  extract_points<cv::Point3f, cv::Point2d>(opoints, ipoints, points);
68  else
69  extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
70 
71  bool result = solve(
72  rotation_matrix, translation, points[0], points[1], points[2],
73  points[3], points[4], points[5], points[6], points[7], points[8],
74  points[9], points[10], points[11], points[12], points[13], points[14],
75  points[15], points[16], points[17], points[18], points[19]);
76  cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
77  cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
78  return result;
79 }
80 
81 #endif
82 
84  Eigen::Ref<Eigen::Matrix3d> R, Eigen::Ref<Eigen::Vector3d> t,
85  Eigen::MatrixXd obj_pts, Eigen::MatrixXd img_pts)
86 {
87  double rotation_matrix[3][3], translation[3];
88  std::vector<double> points;
89  extract_points(obj_pts, img_pts, points);
90  bool result = solve(
91  rotation_matrix, translation, points[0], points[1], points[2],
92  points[3], points[4], points[5], points[6], points[7], points[8],
93  points[9], points[10], points[11], points[12], points[13], points[14],
94  points[15], points[16], points[17], points[18], points[19]);
95  R = Eigen::Map<Eigen::Matrix3d>((double*)rotation_matrix);
96  t = Eigen::Map<Eigen::Vector3d>(translation);
97 
98  return result;
99 }
101  double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0,
102  double Z0, double mu1, double mv1, double X1, double Y1, double Z1,
103  double mu2, double mv2, double X2, double Y2, double Z2, double mu3,
104  double mv3, double X3, double Y3, double Z3)
105 {
106  double Rs[4][3][3], ts[4][3];
107 
108  int n = solve(
109  Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2,
110  Z2);
111 
112  if (n == 0) return false;
113 
114  int ns = 0;
115  double min_reproj = 0;
116  for (int i = 0; i < n; i++)
117  {
118  double X3p =
119  Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
120  double Y3p =
121  Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
122  double Z3p =
123  Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
124  double mu3p = cx + fx * X3p / Z3p;
125  double mv3p = cy + fy * Y3p / Z3p;
126  double reproj =
127  (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
128  if (i == 0 || min_reproj > reproj)
129  {
130  ns = i;
131  min_reproj = reproj;
132  }
133  }
134 
135  for (int i = 0; i < 3; i++)
136  {
137  for (int j = 0; j < 3; j++) R[i][j] = Rs[ns][i][j];
138  t[i] = ts[ns][i];
139  }
140 
141  return true;
142 }
143 
145  double R[4][3][3], double t[4][3], double mu0, double mv0, double X0,
146  double Y0, double Z0, double mu1, double mv1, double X1, double Y1,
147  double Z1, double mu2, double mv2, double X2, double Y2, double Z2)
148 {
149  double mk0, mk1, mk2;
150  double norm;
151 
152  mu0 = inv_fx * mu0 - cx_fx;
153  mv0 = inv_fy * mv0 - cy_fy;
154  norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1);
155  mk0 = 1. / norm;
156  mu0 *= mk0;
157  mv0 *= mk0;
158 
159  mu1 = inv_fx * mu1 - cx_fx;
160  mv1 = inv_fy * mv1 - cy_fy;
161  norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1);
162  mk1 = 1. / norm;
163  mu1 *= mk1;
164  mv1 *= mk1;
165 
166  mu2 = inv_fx * mu2 - cx_fx;
167  mv2 = inv_fy * mv2 - cy_fy;
168  norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1);
169  mk2 = 1. / norm;
170  mu2 *= mk2;
171  mv2 *= mk2;
172 
173  double distances[3];
174  distances[0] = sqrt(
175  (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2));
176  distances[1] = sqrt(
177  (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2));
178  distances[2] = sqrt(
179  (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1));
180 
181  // Calculate angles
182  double cosines[3];
183  cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2;
184  cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2;
185  cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1;
186 
187  double lengths[4][3];
188  int n = solve_for_lengths(lengths, distances, cosines);
189 
190  int nb_solutions = 0;
191  for (int i = 0; i < n; i++)
192  {
193  double M_orig[3][3];
194 
195  M_orig[0][0] = lengths[i][0] * mu0;
196  M_orig[0][1] = lengths[i][0] * mv0;
197  M_orig[0][2] = lengths[i][0] * mk0;
198 
199  M_orig[1][0] = lengths[i][1] * mu1;
200  M_orig[1][1] = lengths[i][1] * mv1;
201  M_orig[1][2] = lengths[i][1] * mk1;
202 
203  M_orig[2][0] = lengths[i][2] * mu2;
204  M_orig[2][1] = lengths[i][2] * mv2;
205  M_orig[2][2] = lengths[i][2] * mk2;
206 
207  if (!align(
208  M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions],
209  t[nb_solutions]))
210  continue;
211 
212  nb_solutions++;
213  }
214 
215  return nb_solutions;
216 }
217 
218 /// Given 3D distances between three points and cosines of 3 angles at the apex,
219 /// calculates
220 /// the lentghs of the line segments connecting projection center (P) and the
221 /// three 3D points (A, B, C).
222 /// Returned distances are for |PA|, |PB|, |PC| respectively.
223 /// Only the solution to the main branch.
224 /// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution
225 /// Classification for the Perspective-Three-Point Problem"
226 /// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003
227 /// \param lengths3D Lengths of line segments up to four solutions.
228 /// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|.
229 /// \param cosines Cosine of the angles /_BPC, /_APC, /_APB.
230 /// \returns Number of solutions.
231 /// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED
232 
234  double lengths[4][3], double distances[3], double cosines[3])
235 {
236  double p = cosines[0] * 2;
237  double q = cosines[1] * 2;
238  double r = cosines[2] * 2;
239 
240  double inv_d22 = 1. / (distances[2] * distances[2]);
241  double a = inv_d22 * (distances[0] * distances[0]);
242  double b = inv_d22 * (distances[1] * distances[1]);
243 
244  double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r;
245  double pr = p * r, pqr = q * pr;
246 
247  // Check reality condition (the four points should not be coplanar)
248  if (p2 + q2 + r2 - pqr - 1 == 0) return 0;
249 
250  double ab = a * b, a_2 = 2 * a;
251 
252  double A = -2 * b + b2 + a2 + 1 + ab * (2 - r2) - a_2;
253 
254  // Check reality condition
255  if (A == 0) return 0;
256 
257  double a_4 = 4 * a;
258 
259  double B =
260  q * (-2 * (ab + a2 + 1 - b) + r2 * ab + a_4) + pr * (b - b2 + ab);
261  double C = q2 + b2 * (r2 + p2 - 2) - b * (p2 + pqr) - ab * (r2 + pqr) +
262  (a2 - a_2) * (2 + q2) + 2;
263  double D =
264  pr * (ab - b2 + b) + q * ((p2 - 2) * b + 2 * (ab - a2) + a_4 - 2);
265  double E = 1 + 2 * (b - a - ab) + b2 - b * p2 + a2;
266 
267  double temp = (p2 * (a - 1 + b) + r2 * (a - 1 - b) + pqr - a * pqr);
268  double b0 = b * temp * temp;
269  // Check reality condition
270  if (b0 == 0) return 0;
271 
272  double real_roots[4];
273  int n = solve_deg4(
274  A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2],
275  real_roots[3]);
276 
277  if (n == 0) return 0;
278 
279  int nb_solutions = 0;
280  double r3 = r2 * r, pr2 = p * r2, r3q = r3 * q;
281  double inv_b0 = 1. / b0;
282 
283  // For each solution of x
284  for (int i = 0; i < n; i++)
285  {
286  double x = real_roots[i];
287 
288  // Check reality condition
289  if (x <= 0) continue;
290 
291  double x2 = x * x;
292 
293  double b1 =
294  ((1 - a - b) * x2 + (q * a - q) * x + 1 - a + b) *
295  (((r3 * (a2 + ab * (2 - r2) - a_2 + b2 - 2 * b + 1)) * x +
296 
297  (r3q * (2 * (b - a2) + a_4 + ab * (r2 - 2) - 2) +
298  pr2 * (1 + a2 + 2 * (ab - a - b) + r2 * (b - b2) + b2))) *
299  x2 +
300 
301  (r3 * (q2 * (1 - 2 * a + a2) + r2 * (b2 - ab) - a_4 +
302  2 * (a2 - b2) + 2) +
303  r * p2 * (b2 + 2 * (ab - b - a) + 1 + a2) +
304  pr2 * q * (a_4 + 2 * (b - ab - a2) - 2 - r2 * b)) *
305  x +
306 
307  2 * r3q * (a_2 - b - a2 + ab - 1) +
308  pr2 * (q2 - a_4 + 2 * (a2 - b2) + r2 * b + q2 * (a2 - a_2) + 2) +
309  p2 * (p * (2 * (ab - a - b) + a2 + b2 + 1) +
310  2 * q * r * (b + a_2 - a2 - ab - 1)));
311 
312  // Check reality condition
313  if (b1 <= 0) continue;
314 
315  double y = inv_b0 * b1;
316  double v = x2 + y * y - x * y * r;
317 
318  if (v <= 0) continue;
319 
320  double Z = distances[2] / sqrt(v);
321  double X = x * Z;
322  double Y = y * Z;
323 
324  lengths[nb_solutions][0] = X;
325  lengths[nb_solutions][1] = Y;
326  lengths[nb_solutions][2] = Z;
327 
328  nb_solutions++;
329  }
330 
331  return nb_solutions;
332 }
333 
335  double M_end[3][3], double X0, double Y0, double Z0, double X1, double Y1,
336  double Z1, double X2, double Y2, double Z2, double R[3][3], double T[3])
337 {
338  // Centroids:
339  double C_start[3], C_end[3];
340  for (int i = 0; i < 3; i++)
341  C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3;
342  C_start[0] = (X0 + X1 + X2) / 3;
343  C_start[1] = (Y0 + Y1 + Y2) / 3;
344  C_start[2] = (Z0 + Z1 + Z2) / 3;
345 
346  // Covariance matrix s:
347  double s[3 * 3];
348  for (int j = 0; j < 3; j++)
349  {
350  s[0 * 3 + j] =
351  (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 -
352  C_end[j] * C_start[0];
353  s[1 * 3 + j] =
354  (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 -
355  C_end[j] * C_start[1];
356  s[2 * 3 + j] =
357  (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 -
358  C_end[j] * C_start[2];
359  }
360 
361  double Qs[16], evs[4], U[16];
362 
363  Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2];
364  Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2];
365  Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0];
366  Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1];
367 
368  Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1];
369  Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2];
370  Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0];
371  Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1];
372  Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2];
373  Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2];
374 
375  jacobi_4x4(Qs, evs, U);
376 
377  // Looking for the largest eigen value:
378  int i_ev = 0;
379  double ev_max = evs[i_ev];
380  for (int i = 1; i < 4; i++)
381  if (evs[i] > ev_max) ev_max = evs[i_ev = i];
382 
383  // Quaternion:
384  double q[4];
385  for (int i = 0; i < 4; i++) q[i] = U[i * 4 + i_ev];
386 
387  double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2],
388  q32 = q[3] * q[3];
389  double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3];
390  double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3];
391  double q2_3 = q[2] * q[3];
392 
393  R[0][0] = q02 + q12 - q22 - q32;
394  R[0][1] = 2. * (q1_2 - q0_3);
395  R[0][2] = 2. * (q1_3 + q0_2);
396 
397  R[1][0] = 2. * (q1_2 + q0_3);
398  R[1][1] = q02 + q22 - q12 - q32;
399  R[1][2] = 2. * (q2_3 - q0_1);
400 
401  R[2][0] = 2. * (q1_3 - q0_2);
402  R[2][1] = 2. * (q2_3 + q0_1);
403  R[2][2] = q02 + q32 - q12 - q22;
404 
405  for (int i = 0; i < 3; i++)
406  T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] +
407  R[i][2] * C_start[2]);
408 
409  return true;
410 }
411 
412 bool mrpt::vision::pnp::p3p::jacobi_4x4(double* A, double* D, double* U)
413 {
414  double B[4], Z[4];
415  double Id[16] = {1., 0., 0., 0., 0., 1., 0., 0.,
416  0., 0., 1., 0., 0., 0., 0., 1.};
417 
418  memcpy(U, Id, 16 * sizeof(double));
419 
420  B[0] = A[0];
421  B[1] = A[5];
422  B[2] = A[10];
423  B[3] = A[15];
424  memcpy(D, B, 4 * sizeof(double));
425  memset(Z, 0, 4 * sizeof(double));
426 
427  for (int iter = 0; iter < 50; iter++)
428  {
429  double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) +
430  fabs(A[7]) + fabs(A[11]);
431 
432  if (sum == 0.0) return true;
433 
434  double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0;
435  for (int i = 0; i < 3; i++)
436  {
437  double* pAij = A + 5 * i + 1;
438  for (int j = i + 1; j < 4; j++)
439  {
440  double Aij = *pAij;
441  double eps_machine = 100.0 * fabs(Aij);
442 
443  if (iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) &&
444  fabs(D[j]) + eps_machine == fabs(D[j]))
445  *pAij = 0.0;
446  else if (fabs(Aij) > tresh)
447  {
448  double hh = D[j] - D[i], t;
449  if (fabs(hh) + eps_machine == fabs(hh))
450  t = Aij / hh;
451  else
452  {
453  double theta = 0.5 * hh / Aij;
454  t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
455  if (theta < 0.0) t = -t;
456  }
457 
458  hh = t * Aij;
459  Z[i] -= hh;
460  Z[j] += hh;
461  D[i] -= hh;
462  D[j] += hh;
463  *pAij = 0.0;
464 
465  double c = 1.0 / sqrt(1 + t * t);
466  double s = t * c;
467  double tau = s / (1.0 + c);
468  for (int k = 0; k <= i - 1; k++)
469  {
470  double g = A[k * 4 + i], h = A[k * 4 + j];
471  A[k * 4 + i] = g - s * (h + g * tau);
472  A[k * 4 + j] = h + s * (g - h * tau);
473  }
474  for (int k = i + 1; k <= j - 1; k++)
475  {
476  double g = A[i * 4 + k], h = A[k * 4 + j];
477  A[i * 4 + k] = g - s * (h + g * tau);
478  A[k * 4 + j] = h + s * (g - h * tau);
479  }
480  for (int k = j + 1; k < 4; k++)
481  {
482  double g = A[i * 4 + k], h = A[j * 4 + k];
483  A[i * 4 + k] = g - s * (h + g * tau);
484  A[j * 4 + k] = h + s * (g - h * tau);
485  }
486  for (int k = 0; k < 4; k++)
487  {
488  double g = U[k * 4 + i], h = U[k * 4 + j];
489  U[k * 4 + i] = g - s * (h + g * tau);
490  U[k * 4 + j] = h + s * (g - h * tau);
491  }
492  }
493  pAij++;
494  }
495  }
496 
497  for (int i = 0; i < 4; i++) B[i] += Z[i];
498  memcpy(D, B, 4 * sizeof(double));
499  memset(Z, 0, 4 * sizeof(double));
500  }
501 
502  return false;
503 }
GLdouble GLdouble t
Definition: glext.h:3695
bool solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Mat &ipoints)
Function to compute pose by P3P using OpenCV.
Definition: p3p.cpp:54
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
GLdouble s
Definition: glext.h:3682
bool jacobi_4x4(double *A, double *D, double *U)
Function used to compute the SVD.
Definition: p3p.cpp:412
GLsizei const GLfloat * points
Definition: glext.h:5414
double inv_fy
Inverse of focal length x.
Definition: p3p.h:211
double cy
Image center x.
Definition: p3p.h:209
bool align(double M_start[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])
Definition: p3p.cpp:334
int solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3])
Helper function to solve()
Definition: p3p.cpp:233
const GLubyte * c
Definition: glext.h:6406
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
GLubyte g
Definition: glext.h:6372
GLubyte GLubyte b
Definition: glext.h:6372
double cy_fy
Inverse of image center point x.
Definition: p3p.h:213
int solve_deg4(double a, double b, double c, double d, double e, double &x0, double &x1, double &x2, double &x3)
Reference : Eric W.
void init_inverse_parameters()
Function to compute inverse parameters of camera intrinsic matrix.
Definition: p3p.cpp:18
double cx
Focal length y.
Definition: p3p.h:208
p3p(double fx, double fy, double cx, double cy)
Constructor for p3p class using C.
Definition: p3p.cpp:35
const GLdouble * v
Definition: glext.h:3684
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
const float R
double fy
Focal length x.
Definition: p3p.h:207
double cx_fx
Inverse of focal length y.
Definition: p3p.h:212
GLenum GLint GLint y
Definition: glext.h:3542
GLenum GLint x
Definition: glext.h:3542
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
double inv_fx
Image center y.
Definition: p3p.h:210
CONTAINER::Scalar norm(const CONTAINER &v)
P3P Pose estimation Algorithm - Eigen Implementation.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019