Main MRPT website > C++ reference for MRPT 1.5.5
ppnp.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 <iostream>
13 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
14 #include <Eigen/Dense>
15 #include <Eigen/SVD>
16 
17 #include "ppnp.h"
18 
19 mrpt::vision::pnp::ppnp::ppnp(const Eigen::MatrixXd& obj_pts, const Eigen::MatrixXd& img_pts, const Eigen::MatrixXd& cam_intrinsic)
20 {
21  P = img_pts;
22  S = obj_pts;
23  C = cam_intrinsic;
24 }
25 
26 bool mrpt::vision::pnp::ppnp::compute_pose(Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
27 {
28  double tol=0.00001;
29 
30  Eigen::MatrixXd I=Eigen::MatrixXd::Identity(n, n), A(n,n), Y(n,3), E(n,3), E_old(n,3), U,V, I3 =Eigen::MatrixXd::Identity(3, 3), PR, Z=Eigen::MatrixXd::Zero(n, n);
31  Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);
32 
33  e.fill(1);
34  II.fill(1.0/((double)n));
35 
36  A=I-e*e.transpose()/n;
37 
38  double err=std::numeric_limits<double>::infinity();
39 
40  E_old.fill(1000);
41 
42  int cnt =0;
43 
44  while(err>tol)
45  {
46 
47  Eigen::JacobiSVD<Eigen::MatrixXd> svd(P.transpose()*Z*A*S, Eigen::ComputeThinU | Eigen::ComputeThinV);
48  U=svd.matrixU();
49  V=svd.matrixV();
50 
51  I3(2,2) = (U*V.transpose()).determinant();
52  R=U*I3*V.transpose();
53  PR = P*R;
54 
55  c=(S-Z*PR).transpose()*II;
56 
57  Y=S-e*c.transpose();
58 
59  Zmindiag=((PR*Y.transpose()).diagonal()).array() / ((P.array()*P.array()).rowwise().sum()).array();
60 
61  for (int i = 0; i < n; i++)
62  if (Zmindiag(i) < 0)
63  Zmindiag(i) = 0;
64 
65 
66  Z=Zmindiag.asDiagonal();
67 
68  E=Y-Z*PR;
69 
70  err=(E-E_old).norm();
71 
72  E_old=E;
73 
74  cnt ++;
75 
76  }
77 
78  t=-R*c;
79 
80  return 1;
81 
82 
83 }
GLdouble GLdouble t
Definition: glext.h:3610
Eigen::MatrixXd P
Definition: ppnp.h:41
Procrustes - PnP.
Eigen::MatrixXd S
Image points in pixels.
Definition: ppnp.h:42
ppnp(const Eigen::MatrixXd &obj_pts, const Eigen::MatrixXd &img_pts, const Eigen::MatrixXd &cam_intrinsic)
Constructor for the P-PnP class.
Definition: ppnp.cpp:19
GLenum GLsizei n
Definition: glext.h:4618
bool compute_pose(Eigen::Matrix3d &R, Eigen::Vector3d &t, int n)
Function to compute pose.
Definition: ppnp.cpp:26
const GLubyte * c
Definition: glext.h:5590
GLsizei GLboolean transpose
Definition: glext.h:3937
Eigen::MatrixXd C
Object points in Camera Co-ordinate system.
Definition: ppnp.h:43
const float R
CONTAINER::Scalar norm(const CONTAINER &v)



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