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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020