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


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019