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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019