Main MRPT website > C++ reference for MRPT 1.5.5
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-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 
12 #include <mrpt/config.h>
13 #include <mrpt/vision/utils.h>
14 #include <mrpt/vision/pnp_algos.h>
15 
16 
17 // Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable 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/utils/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(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){
46  try{
47  #if MRPT_HAS_OPENCV==1
48 
49  // Input 2d/3d correspondences and camera intrinsic matrix
50  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
51 
52  // Check for consistency of input matrix dimensions
53  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
54  throw(2);
55  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
56  throw(3);
57 
58  if(obj_pts.rows() < obj_pts.cols())
59  {
60  cam_in_eig=cam_intrinsic.transpose();
61  img_pts_eig=img_pts.transpose().block(0,0,n,2);
62  obj_pts_eig=obj_pts.transpose();
63  }
64  else
65  {
66  cam_in_eig=cam_intrinsic;
67  img_pts_eig=img_pts.block(0,0,n,2);
68  obj_pts_eig=obj_pts;
69  }
70 
71  // Output pose
72  Eigen::Matrix3d R_eig;
73  Eigen::MatrixXd t_eig;
74 
75  // Compute pose
76  cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);
77 
78  cv::eigen2cv(cam_in_eig, cam_in_cv);
79  cv::eigen2cv(img_pts_eig, img_pts_cv);
80  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
81 
82  mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
83  bool ret = d.compute_pose(R_cv,t_cv);
84 
85  cv::cv2eigen(R_cv, R_eig);
86  cv::cv2eigen(t_cv, t_eig);
87 
88  Eigen::Quaterniond q(R_eig);
89 
90  pose_mat << t_eig,q.vec();
91 
92  return ret;
93 
94  #else
95  throw(-1);
96  #endif
97  }
98  catch(int e)
99  {
100  switch(e)
101  {
102  case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl; break;
103  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
104  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
105  }
106  return false;
107  }
108 }
109 
110 bool mrpt::vision::pnp::CPnP::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){
111  try{
112  #if MRPT_HAS_OPENCV==1
113 
114  // Input 2d/3d correspondences and camera intrinsic matrix
115  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
116 
117  // Check for consistency of input matrix dimensions
118  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
119  throw(2);
120  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
121  throw(3);
122 
123  if(obj_pts.rows() < obj_pts.cols())
124  {
125  cam_in_eig=cam_intrinsic.transpose();
126  img_pts_eig=img_pts.transpose().block(0,0,n,2);
127  obj_pts_eig=obj_pts.transpose();
128  }
129  else
130  {
131  cam_in_eig=cam_intrinsic;
132  img_pts_eig=img_pts.block(0,0,n,2);
133  obj_pts_eig=obj_pts;
134  }
135 
136  // Output pose
137  Eigen::Matrix3d R_eig;
138  Eigen::MatrixXd t_eig;
139 
140  // Compute pose
141  cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;
142 
143  cv::eigen2cv(cam_in_eig, cam_in_cv);
144  cv::eigen2cv(img_pts_eig, img_pts_cv);
145  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
146 
147  mrpt::vision::pnp::epnp e(cam_in_cv, obj_pts_cv, img_pts_cv);
148  e.compute_pose(R_cv,t_cv);
149 
150  cv::cv2eigen(R_cv, R_eig);
151  cv::cv2eigen(t_cv, t_eig);
152 
153  Eigen::Quaterniond q(R_eig);
154 
155  pose_mat << t_eig,q.vec();
156 
157  return true;
158 
159  #else
160  throw(-1);
161  #endif
162  }
163  catch(int e){
164  switch(e)
165  {
166  case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl; break;
167  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
168  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
169  }
170  return false;
171  }
172 }
173 
174 bool mrpt::vision::pnp::CPnP::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){
175  try{
176  #if MRPT_HAS_OPENCV==1
177 
178  // Input 2d/3d correspondences and camera intrinsic matrix
179  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
180 
181  // Check for consistency of input matrix dimensions
182  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
183  throw(2);
184  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
185  throw(3);
186 
187  if(obj_pts.rows() < obj_pts.cols())
188  {
189  cam_in_eig=cam_intrinsic.transpose();
190  img_pts_eig=img_pts.transpose().block(0,0,n,2);
191  obj_pts_eig=obj_pts.transpose();
192  }
193  else
194  {
195  cam_in_eig=cam_intrinsic;
196  img_pts_eig=img_pts.block(0,0,n,2);
197  obj_pts_eig=obj_pts;
198  }
199 
200  // Output pose
201  Eigen::Matrix3d R_eig;
202  Eigen::MatrixXd t_eig;
203 
204  // Compute pose
205  cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;
206 
207  cv::eigen2cv(cam_in_eig, cam_in_cv);
208  cv::eigen2cv(img_pts_eig, img_pts_cv);
209  cv::eigen2cv(obj_pts_eig, obj_pts_cv);
210 
211  mrpt::vision::pnp::upnp u(cam_in_cv, obj_pts_cv, img_pts_cv);
212  u.compute_pose(R_cv,t_cv);
213 
214  cv::cv2eigen(R_cv, R_eig);
215  cv::cv2eigen(t_cv, t_eig);
216 
217  Eigen::Quaterniond q(R_eig);
218 
219  pose_mat << t_eig,q.vec();
220 
221  return true;
222  #else
223  throw(-1);
224  #endif
225  }
226  catch(int e)
227  {
228  switch(e)
229  {
230  case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl; break;
231  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
232  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
233  }
234  return false;
235  }
236 }
237 
238 
239 bool mrpt::vision::pnp::CPnP::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){
240 
241  try{
242  // Input 2d/3d correspondences and camera intrinsic matrix
243  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
244 
245  // Check for consistency of input matrix dimensions
246  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
247  throw(2);
248  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
249  throw(3);
250 
251  if(obj_pts.rows() < obj_pts.cols())
252  {
253  cam_in_eig=cam_intrinsic.transpose();
254  img_pts_eig=img_pts.transpose().block(0,0,n,2);
255  obj_pts_eig=obj_pts.transpose();
256  }
257  else
258  {
259  cam_in_eig=cam_intrinsic;
260  img_pts_eig=img_pts.block(0,0,n,2);
261  obj_pts_eig=obj_pts;
262  }
263 
264  // Output pose
265  Eigen::Matrix3d R;
266  Eigen::Vector3d t;
267 
268  // Compute pose
269  mrpt::vision::pnp::p3p p(cam_in_eig);
270  bool ret = p.solve(R,t, obj_pts_eig, img_pts_eig);
271 
272  Eigen::Quaterniond q(R);
273 
274  pose_mat << t,q.vec();
275 
276  return ret;
277  }
278  catch(int e)
279  {
280  switch(e)
281  {
282  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
283  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
284  }
285  return false;
286  }
287 }
288 
289 
290 bool mrpt::vision::pnp::CPnP::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){
291  try{
292  // Input 2d/3d correspondences and camera intrinsic matrix
293  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
294 
295  // Check for consistency of input matrix dimensions
296  if (img_pts.rows() != obj_pts.rows() || 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();
305  obj_pts_eig=obj_pts.transpose();
306  }
307  else
308  {
309  cam_in_eig=cam_intrinsic;
310  img_pts_eig=img_pts;
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::rpnp r(obj_pts_eig, img_pts_eig, cam_in_eig, n);
320  bool ret = r.compute_pose(R,t);
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: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
333  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
334  }
335  return false;
336  }
337 }
338 
339 bool mrpt::vision::pnp::CPnP::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)
340 {
341  try{
342  // Input 2d/3d correspondences and camera intrinsic matrix
343  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
344 
345  // Check for consistency of input matrix dimensions
346  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
347  throw(2);
348  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
349  throw(3);
350 
351  if(obj_pts.rows() < obj_pts.cols())
352  {
353  cam_in_eig=cam_intrinsic.transpose();
354  img_pts_eig=img_pts.transpose();
355  obj_pts_eig=obj_pts.transpose();
356  }
357  else
358  {
359  cam_in_eig=cam_intrinsic;
360  img_pts_eig=img_pts;
361  obj_pts_eig=obj_pts;
362  }
363 
364  // Output pose
365  Eigen::Matrix3d R;
366  Eigen::Vector3d t;
367 
368  // Compute pose
369  mrpt::vision::pnp::ppnp p(obj_pts_eig,img_pts_eig, cam_in_eig);
370 
371  bool ret = p.compute_pose(R,t,n);
372 
373  Eigen::Quaterniond q(R);
374 
375  pose_mat << t,q.vec();
376 
377  return ret;
378  }
379  catch(int e)
380  {
381  switch(e)
382  {
383  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
384  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
385  }
386  return false;
387  }
388 }
389 
390 bool mrpt::vision::pnp::CPnP::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)
391 {
392  try{
393  // Input 2d/3d correspondences and camera intrinsic matrix
394  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
395 
396  // Check for consistency of input matrix dimensions
397  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
398  throw(2);
399  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
400  throw(3);
401 
402  if(obj_pts.rows() < obj_pts.cols())
403  {
404  cam_in_eig=cam_intrinsic.transpose();
405  img_pts_eig=img_pts.transpose().block(0,0,n,2);
406  obj_pts_eig=obj_pts.transpose();
407  }
408  else
409  {
410  cam_in_eig=cam_intrinsic;
411  img_pts_eig=img_pts.block(0,0,n,2);
412  obj_pts_eig=obj_pts;
413  }
414 
415  // Output pose
416  Eigen::Matrix3d R;
417  Eigen::Vector3d t;
418 
419  // Compute pose
420  mrpt::vision::pnp::posit p(obj_pts_eig,img_pts_eig, cam_in_eig, n);
421 
422  bool ret = p.compute_pose(R,t);
423 
424  Eigen::Quaterniond q(R);
425 
426  pose_mat << t,q.vec();
427 
428  return ret;
429  }
430  catch(int e)
431  {
432  switch(e)
433  {
434  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
435  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
436  }
437  return false;
438  }
439 
440 }
441 
442 bool mrpt::vision::pnp::CPnP::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)
443 {
444  try{
445  // Input 2d/3d correspondences and camera intrinsic matrix
446  Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;
447 
448  // Check for consistency of input matrix dimensions
449  if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
450  throw(2);
451  else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
452  throw(3);
453 
454  if(obj_pts.rows() < obj_pts.cols())
455  {
456  cam_in_eig=cam_intrinsic.transpose();
457  img_pts_eig=img_pts.transpose();
458  obj_pts_eig=obj_pts.transpose();
459  }
460  else
461  {
462  cam_in_eig=cam_intrinsic;
463  img_pts_eig=img_pts;
464  obj_pts_eig=obj_pts;
465  }
466 
467  // Output pose
468  Eigen::Matrix3d R;
469  Eigen::Vector3d t;
470 
471  // Compute pose
472  mrpt::vision::pnp::lhm l(obj_pts_eig, img_pts_eig, cam_intrinsic, n);
473 
474  bool ret = l.compute_pose(R,t);
475 
476  Eigen::Quaterniond q(R);
477 
478  pose_mat<<t,q.vec();
479 
480  return ret;
481  }
482  catch(int e)
483  {
484  switch(e)
485  {
486  case 2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl; break;
487  case 3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl; break;
488  }
489  return false;
490  }
491 }
Unified PnP - Eigen Wrapper for OpenCV function.
GLdouble GLdouble t
Definition: glext.h:3610
bool compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV function for computing pose.
Procrustes - PnP.
PnP Algorithms toolkit for MRPT.
void compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV wrapper to compute pose.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
GLenum GLsizei n
Definition: glext.h:4618
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:174
double compute_pose(cv::Mat &R, cv::Mat &t)
Function to compute pose.
Robust - PnP class definition for computing pose.
Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation.
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:339
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:290
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:110
Efficient PnP - Eigen Wrapper for OpenCV calib3d implementation.
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:442
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Function to compute pose using LHM PnP algorithm.
Definition: lhm.cpp:197
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
const float R
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:239
Pose from Orthogonality and Scaling (POSIT) - Eigen Implementation.
Direct Least Squares (DLS) - Eigen wrapper for OpenCV Implementation.
GLfloat GLfloat p
Definition: glext.h:5587
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
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:390
P3P Pose estimation Algorithm - Eigen Implementation.



Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019