MRPT  1.9.9
pnp_algos.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 
12 #include <mrpt/config.h>
13 #include <mrpt/vision/utils.h>
14 #include <mrpt/vision/pnp_algos.h>
15 
16 // Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable
17 // PNP classes
18 #include <mrpt/config.h>
19 
20 #if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM < 0x240
21 #undef MRPT_HAS_OPENCV
22 #define MRPT_HAS_OPENCV 0
23 #endif
24 
25 #include <iostream>
26 
27 #include <mrpt/math/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
28 #include <Eigen/Core>
29 #include <Eigen/Dense>
30 
31 #include <mrpt/otherlibs/do_opencv_includes.h>
32 #if MRPT_HAS_OPENCV
33 #include <opencv2/core/eigen.hpp>
34 #endif
35 
36 #include "dls.h"
37 #include "epnp.h"
38 #include "upnp.h"
39 #include "p3p.h"
40 #include "ppnp.h"
41 #include "posit.h"
42 #include "lhm.h"
43 #include "rpnp.h"
44 
45 bool mrpt::vision::pnp::CPnP::dls(
46  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
47  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
48  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
49  Eigen::Ref<Eigen::MatrixXd> pose_mat)
50 {
51  try
52  {
53 #if MRPT_HAS_OPENCV == 1
54 
55  // Input 2d/3d correspondences and camera intrinsic matrix
56  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
57 
58  // Check for consistency of input matrix dimensions
59  if (img_pts.rows() != obj_pts.rows() ||
60  img_pts.cols() != obj_pts.cols())
61  throw(2);
62  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
63  throw(3);
64 
65  if (obj_pts.rows() < obj_pts.cols())
66  {
67  cam_in_eig = cam_intrinsic.transpose();
68  img_pts_eig = img_pts.transpose().block(0, 0, n, 2);
69  obj_pts_eig = obj_pts.transpose();
70  }
71  else
72  {
73  cam_in_eig = cam_intrinsic;
74  img_pts_eig = img_pts.block(0, 0, n, 2);
75  obj_pts_eig = obj_pts;
76  }
77 
78  // Output pose
79  Eigen::Matrix3d R_eig;
80  Eigen::MatrixXd t_eig;
81 
82  // Compute pose
83  cv::Mat cam_in_cv(3, 3, CV_32F), img_pts_cv(2, n, CV_32F),
84  obj_pts_cv(3, n, CV_32F), R_cv(3, 3, CV_32F), t_cv(3, 1, CV_32F);
85 
86  cv::eigen2cv(cam_in_eig, cam_in_cv);
87  cv::eigen2cv(img_pts_eig, img_pts_cv);
88  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
89 
90  mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
91  bool ret = d.compute_pose(R_cv, t_cv);
92 
93  cv::cv2eigen(R_cv, R_eig);
94  cv::cv2eigen(t_cv, t_eig);
95 
96  Eigen::Quaterniond q(R_eig);
97 
98  pose_mat << t_eig, q.vec();
99 
100  return ret;
101 
102 #else
103  throw(-1);
104 #endif
105  }
106  catch (int e)
107  {
108  switch (e)
109  {
110  case -1:
111  std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
112  break;
113  case 2:
114  std::cout << "2d/3d correspondences mismatch\n Check dimension "
115  "of obj_pts and img_pts"
116  << std::endl;
117  break;
118  case 3:
119  std::cout
120  << "Camera intrinsic matrix does not have 3x3 dimensions "
121  << std::endl;
122  break;
123  }
124  return false;
125  }
126 }
127 
128 bool mrpt::vision::pnp::CPnP::epnp(
129  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
130  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
131  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
132  Eigen::Ref<Eigen::MatrixXd> pose_mat)
133 {
134  try
135  {
136 #if MRPT_HAS_OPENCV == 1
137 
138  // Input 2d/3d correspondences and camera intrinsic matrix
139  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
140 
141  // Check for consistency of input matrix dimensions
142  if (img_pts.rows() != obj_pts.rows() ||
143  img_pts.cols() != obj_pts.cols())
144  throw(2);
145  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
146  throw(3);
147 
148  if (obj_pts.rows() < obj_pts.cols())
149  {
150  cam_in_eig = cam_intrinsic.transpose();
151  img_pts_eig = img_pts.transpose().block(0, 0, n, 2);
152  obj_pts_eig = obj_pts.transpose();
153  }
154  else
155  {
156  cam_in_eig = cam_intrinsic;
157  img_pts_eig = img_pts.block(0, 0, n, 2);
158  obj_pts_eig = obj_pts;
159  }
160 
161  // Output pose
162  Eigen::Matrix3d R_eig;
163  Eigen::MatrixXd t_eig;
164 
165  // Compute pose
166  cv::Mat cam_in_cv(3, 3, CV_32F), img_pts_cv(2, n, CV_32F),
167  obj_pts_cv(3, n, CV_32F), R_cv, t_cv;
168 
169  cv::eigen2cv(cam_in_eig, cam_in_cv);
170  cv::eigen2cv(img_pts_eig, img_pts_cv);
171  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
172 
173  mrpt::vision::pnp::epnp e(cam_in_cv, obj_pts_cv, img_pts_cv);
174  e.compute_pose(R_cv, t_cv);
175 
176  cv::cv2eigen(R_cv, R_eig);
177  cv::cv2eigen(t_cv, t_eig);
178 
179  Eigen::Quaterniond q(R_eig);
180 
181  pose_mat << t_eig, q.vec();
182 
183  return true;
184 
185 #else
186  throw(-1);
187 #endif
188  }
189  catch (int e)
190  {
191  switch (e)
192  {
193  case -1:
194  std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
195  break;
196  case 2:
197  std::cout << "2d/3d correspondences mismatch\n Check dimension "
198  "of obj_pts and img_pts"
199  << std::endl;
200  break;
201  case 3:
202  std::cout
203  << "Camera intrinsic matrix does not have 3x3 dimensions "
204  << std::endl;
205  break;
206  }
207  return false;
208  }
209 }
210 
211 bool mrpt::vision::pnp::CPnP::upnp(
212  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
213  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
214  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
215  Eigen::Ref<Eigen::MatrixXd> pose_mat)
216 {
217  try
218  {
219 #if MRPT_HAS_OPENCV == 1
220 
221  // Input 2d/3d correspondences and camera intrinsic matrix
222  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
223 
224  // Check for consistency of input matrix dimensions
225  if (img_pts.rows() != obj_pts.rows() ||
226  img_pts.cols() != obj_pts.cols())
227  throw(2);
228  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
229  throw(3);
230 
231  if (obj_pts.rows() < obj_pts.cols())
232  {
233  cam_in_eig = cam_intrinsic.transpose();
234  img_pts_eig = img_pts.transpose().block(0, 0, n, 2);
235  obj_pts_eig = obj_pts.transpose();
236  }
237  else
238  {
239  cam_in_eig = cam_intrinsic;
240  img_pts_eig = img_pts.block(0, 0, n, 2);
241  obj_pts_eig = obj_pts;
242  }
243 
244  // Output pose
245  Eigen::Matrix3d R_eig;
246  Eigen::MatrixXd t_eig;
247 
248  // Compute pose
249  cv::Mat cam_in_cv(3, 3, CV_32F), img_pts_cv(2, n, CV_32F),
250  obj_pts_cv(3, n, CV_32F), R_cv, t_cv;
251 
252  cv::eigen2cv(cam_in_eig, cam_in_cv);
253  cv::eigen2cv(img_pts_eig, img_pts_cv);
254  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
255 
256  mrpt::vision::pnp::upnp u(cam_in_cv, obj_pts_cv, img_pts_cv);
257  u.compute_pose(R_cv, t_cv);
258 
259  cv::cv2eigen(R_cv, R_eig);
260  cv::cv2eigen(t_cv, t_eig);
261 
262  Eigen::Quaterniond q(R_eig);
263 
264  pose_mat << t_eig, q.vec();
265 
266  return true;
267 #else
268  throw(-1);
269 #endif
270  }
271  catch (int e)
272  {
273  switch (e)
274  {
275  case -1:
276  std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
277  break;
278  case 2:
279  std::cout << "2d/3d correspondences mismatch\n Check dimension "
280  "of obj_pts and img_pts"
281  << std::endl;
282  break;
283  case 3:
284  std::cout
285  << "Camera intrinsic matrix does not have 3x3 dimensions "
286  << std::endl;
287  break;
288  }
289  return false;
290  }
291 }
292 
293 bool mrpt::vision::pnp::CPnP::p3p(
294  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
295  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
296  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
297  Eigen::Ref<Eigen::MatrixXd> pose_mat)
298 {
299  try
300  {
301  // Input 2d/3d correspondences and camera intrinsic matrix
302  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
303 
304  // Check for consistency of input matrix dimensions
305  if (img_pts.rows() != obj_pts.rows() ||
306  img_pts.cols() != obj_pts.cols())
307  throw(2);
308  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
309  throw(3);
310 
311  if (obj_pts.rows() < obj_pts.cols())
312  {
313  cam_in_eig = cam_intrinsic.transpose();
314  img_pts_eig = img_pts.transpose().block(0, 0, n, 2);
315  obj_pts_eig = obj_pts.transpose();
316  }
317  else
318  {
319  cam_in_eig = cam_intrinsic;
320  img_pts_eig = img_pts.block(0, 0, n, 2);
321  obj_pts_eig = obj_pts;
322  }
323 
324  // Output pose
325  Eigen::Matrix3d R;
326  Eigen::Vector3d t;
327 
328  // Compute pose
329  mrpt::vision::pnp::p3p p(cam_in_eig);
330  bool ret = p.solve(R, t, obj_pts_eig, img_pts_eig);
331 
332  Eigen::Quaterniond q(R);
333 
334  pose_mat << t, q.vec();
335 
336  return ret;
337  }
338  catch (int e)
339  {
340  switch (e)
341  {
342  case 2:
343  std::cout << "2d/3d correspondences mismatch\n Check dimension "
344  "of obj_pts and img_pts"
345  << std::endl;
346  break;
347  case 3:
348  std::cout
349  << "Camera intrinsic matrix does not have 3x3 dimensions "
350  << std::endl;
351  break;
352  }
353  return false;
354  }
355 }
356 
357 bool mrpt::vision::pnp::CPnP::rpnp(
358  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
359  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
360  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
361  Eigen::Ref<Eigen::MatrixXd> pose_mat)
362 {
363  try
364  {
365  // Input 2d/3d correspondences and camera intrinsic matrix
366  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
367 
368  // Check for consistency of input matrix dimensions
369  if (img_pts.rows() != obj_pts.rows() ||
370  img_pts.cols() != obj_pts.cols())
371  throw(2);
372  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
373  throw(3);
374 
375  if (obj_pts.rows() < obj_pts.cols())
376  {
377  cam_in_eig = cam_intrinsic.transpose();
378  img_pts_eig = img_pts.transpose();
379  obj_pts_eig = obj_pts.transpose();
380  }
381  else
382  {
383  cam_in_eig = cam_intrinsic;
384  img_pts_eig = img_pts;
385  obj_pts_eig = obj_pts;
386  }
387 
388  // Output pose
389  Eigen::Matrix3d R;
390  Eigen::Vector3d t;
391 
392  // Compute pose
393  mrpt::vision::pnp::rpnp r(obj_pts_eig, img_pts_eig, cam_in_eig, n);
394  bool ret = r.compute_pose(R, t);
395 
396  Eigen::Quaterniond q(R);
397 
398  pose_mat << t, q.vec();
399 
400  return ret;
401  }
402  catch (int e)
403  {
404  switch (e)
405  {
406  case 2:
407  std::cout << "2d/3d correspondences mismatch\n Check dimension "
408  "of obj_pts and img_pts"
409  << std::endl;
410  break;
411  case 3:
412  std::cout
413  << "Camera intrinsic matrix does not have 3x3 dimensions "
414  << std::endl;
415  break;
416  }
417  return false;
418  }
419 }
420 
421 bool mrpt::vision::pnp::CPnP::ppnp(
422  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
423  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
424  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
425  Eigen::Ref<Eigen::MatrixXd> pose_mat)
426 {
427  try
428  {
429  // Input 2d/3d correspondences and camera intrinsic matrix
430  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
431 
432  // Check for consistency of input matrix dimensions
433  if (img_pts.rows() != obj_pts.rows() ||
434  img_pts.cols() != obj_pts.cols())
435  throw(2);
436  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
437  throw(3);
438 
439  if (obj_pts.rows() < obj_pts.cols())
440  {
441  cam_in_eig = cam_intrinsic.transpose();
442  img_pts_eig = img_pts.transpose();
443  obj_pts_eig = obj_pts.transpose();
444  }
445  else
446  {
447  cam_in_eig = cam_intrinsic;
448  img_pts_eig = img_pts;
449  obj_pts_eig = obj_pts;
450  }
451 
452  // Output pose
453  Eigen::Matrix3d R;
454  Eigen::Vector3d t;
455 
456  // Compute pose
457  mrpt::vision::pnp::ppnp p(obj_pts_eig, img_pts_eig, cam_in_eig);
458 
459  bool ret = p.compute_pose(R, t, n);
460 
461  Eigen::Quaterniond q(R);
462 
463  pose_mat << t, q.vec();
464 
465  return ret;
466  }
467  catch (int e)
468  {
469  switch (e)
470  {
471  case 2:
472  std::cout << "2d/3d correspondences mismatch\n Check dimension "
473  "of obj_pts and img_pts"
474  << std::endl;
475  break;
476  case 3:
477  std::cout
478  << "Camera intrinsic matrix does not have 3x3 dimensions "
479  << std::endl;
480  break;
481  }
482  return false;
483  }
484 }
485 
486 bool mrpt::vision::pnp::CPnP::posit(
487  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
488  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
489  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
490  Eigen::Ref<Eigen::MatrixXd> pose_mat)
491 {
492  try
493  {
494  // Input 2d/3d correspondences and camera intrinsic matrix
495  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
496 
497  // Check for consistency of input matrix dimensions
498  if (img_pts.rows() != obj_pts.rows() ||
499  img_pts.cols() != obj_pts.cols())
500  throw(2);
501  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
502  throw(3);
503 
504  if (obj_pts.rows() < obj_pts.cols())
505  {
506  cam_in_eig = cam_intrinsic.transpose();
507  img_pts_eig = img_pts.transpose().block(0, 0, n, 2);
508  obj_pts_eig = obj_pts.transpose();
509  }
510  else
511  {
512  cam_in_eig = cam_intrinsic;
513  img_pts_eig = img_pts.block(0, 0, n, 2);
514  obj_pts_eig = obj_pts;
515  }
516 
517  // Output pose
518  Eigen::Matrix3d R;
519  Eigen::Vector3d t;
520 
521  // Compute pose
522  mrpt::vision::pnp::posit p(obj_pts_eig, img_pts_eig, cam_in_eig, n);
523 
524  bool ret = p.compute_pose(R, t);
525 
526  Eigen::Quaterniond q(R);
527 
528  pose_mat << t, q.vec();
529 
530  return ret;
531  }
532  catch (int e)
533  {
534  switch (e)
535  {
536  case 2:
537  std::cout << "2d/3d correspondences mismatch\n Check dimension "
538  "of obj_pts and img_pts"
539  << std::endl;
540  break;
541  case 3:
542  std::cout
543  << "Camera intrinsic matrix does not have 3x3 dimensions "
544  << std::endl;
545  break;
546  }
547  return false;
548  }
549 }
550 
551 bool mrpt::vision::pnp::CPnP::lhm(
552  const Eigen::Ref<Eigen::MatrixXd> obj_pts,
553  const Eigen::Ref<Eigen::MatrixXd> img_pts, int n,
554  const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic,
555  Eigen::Ref<Eigen::MatrixXd> pose_mat)
556 {
557  try
558  {
559  // Input 2d/3d correspondences and camera intrinsic matrix
560  Eigen::MatrixXd cam_in_eig, img_pts_eig, obj_pts_eig;
561 
562  // Check for consistency of input matrix dimensions
563  if (img_pts.rows() != obj_pts.rows() ||
564  img_pts.cols() != obj_pts.cols())
565  throw(2);
566  else if (cam_intrinsic.rows() != 3 || cam_intrinsic.cols() != 3)
567  throw(3);
568 
569  if (obj_pts.rows() < obj_pts.cols())
570  {
571  cam_in_eig = cam_intrinsic.transpose();
572  img_pts_eig = img_pts.transpose();
573  obj_pts_eig = obj_pts.transpose();
574  }
575  else
576  {
577  cam_in_eig = cam_intrinsic;
578  img_pts_eig = img_pts;
579  obj_pts_eig = obj_pts;
580  }
581 
582  // Output pose
583  Eigen::Matrix3d R;
584  Eigen::Vector3d t;
585 
586  // Compute pose
587  mrpt::vision::pnp::lhm l(obj_pts_eig, img_pts_eig, cam_intrinsic, n);
588 
589  bool ret = l.compute_pose(R, t);
590 
591  Eigen::Quaterniond q(R);
592 
593  pose_mat << t, q.vec();
594 
595  return ret;
596  }
597  catch (int e)
598  {
599  switch (e)
600  {
601  case 2:
602  std::cout << "2d/3d correspondences mismatch\n Check dimension "
603  "of obj_pts and img_pts"
604  << std::endl;
605  break;
606  case 3:
607  std::cout
608  << "Camera intrinsic matrix does not have 3x3 dimensions "
609  << std::endl;
610  break;
611  }
612  return false;
613  }
614 }
upnp.h
Unified PnP - Eigen Wrapper for OpenCV function.
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::vision::pnp::dls::compute_pose
bool compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV function for computing pose.
ppnp.h
Procrustes - PnP.
types_math.h
pnp_algos.h
PnP Algorithms toolkit for MRPT.
mrpt::vision::pnp::epnp::compute_pose
void compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV wrapper to compute pose.
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::vision::pnp::CPnP::upnp
bool upnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Unified-PnP (UPnP) : Algorithm to compute pose from unknown camera intrinsic matrix ...
Definition: pnp_algos.cpp:211
mrpt::vision::pnp::upnp::compute_pose
double compute_pose(cv::Mat &R, cv::Mat &t)
Function to compute pose.
rpnp.h
Robust - PnP class definition for computing pose.
lhm.h
Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation.
mrpt::vision::pnp::CPnP::ppnp
bool ppnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Procrustes-PnP(PPnP) Algorithm : Iterative SVD based algorithm
Definition: pnp_algos.cpp:421
mrpt::vision::pnp::CPnP::rpnp
bool rpnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Robust (R-PnP)- A closed form solution with intermediate P3P computations
Definition: pnp_algos.cpp:357
mrpt::vision::pnp::CPnP::epnp
bool epnp(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Efficient-PnP (EPnP) - Algorithm takes 4 control points based on n points and uses the control point...
Definition: pnp_algos.cpp:128
epnp.h
Efficient PnP - Eigen Wrapper for OpenCV calib3d implementation.
mrpt::vision::pnp::CPnP::lhm
bool lhm(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Lu-Hager-Mjolsness(LHM)-PnP algorithm : Iterative algorithm to reduce object space error ...
Definition: pnp_algos.cpp:551
mrpt::vision::pnp::lhm::compute_pose
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Function to compute pose using LHM PnP algorithm.
Definition: lhm.cpp:194
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
R
const float R
Definition: CKinematicChain.cpp:138
mrpt::vision::pnp::CPnP::p3p
bool p3p(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
P3P - A closed form solution for n=3, 2D-3D correspondences
Definition: pnp_algos.cpp:293
posit.h
Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation.
dls.h
Direct Least Squares (DLS) - Eigen wrapper for OpenCV Implementation.
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::vision::pnp::CPnP::dls
bool dls(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Direct Least Squares (DLS) - PnP : Algorithm formulates position as a function of rotation...
Definition: pnp_algos.cpp:45
mrpt::vision::pnp::CPnP::posit
bool posit(const Eigen::Ref< Eigen::MatrixXd > obj_pts, const Eigen::Ref< Eigen::MatrixXd > img_pts, int n, const Eigen::Ref< Eigen::MatrixXd > cam_intrinsic, Eigen::Ref< Eigen::MatrixXd > pose_mat)
Pose from Orthogoanlity and Scaling :Iterative (POSIT) - A Geometric algorithm to compute scale and ...
Definition: pnp_algos.cpp:486
p3p.h
P3P Pose estimation Algorithm - Eigen Implementation.


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020