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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019