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


Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020