MRPT  1.9.9
lhm.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-2019, 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 <iostream>
11 #include "vision-precomp.h" // Precompiled headers
12 
13 //#include <mrpt/math/types_math.h> // Eigen must be included first via MRPT to
14 // enable the plugin system
15 #include <Eigen/Dense>
16 #include <Eigen/SVD>
17 #include <Eigen/StdVector>
18 
19 #include "lhm.h"
20 using namespace mrpt::vision::pnp;
21 
23  Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_,
24  int n0)
25  : F(n0)
26 {
27  obj_pts = obj_pts_;
28  img_pts = img_pts_;
29  cam_intrinsic = cam_;
30  n = n0;
31 
32  // Store obj_pts as 3XN and img_projections as 2XN matrices
33  P = obj_pts.transpose();
34  Q = Eigen::MatrixXd::Ones(3, n);
35 
36  Q = img_pts.transpose();
37 
38  t.setZero();
39 }
40 
42 {
43  Eigen::Vector3d sum_;
44  sum_.setZero();
45  for (int i = 0; i < n; i++) sum_ += F[i] * R * P.col(i);
46  t = G * sum_;
47 }
48 
49 void lhm::xform()
50 {
51  for (int i = 0; i < n; i++) Q.col(i) = R * P.col(i) + t;
52 }
53 
54 Eigen::Matrix4d lhm::qMatQ(Eigen::VectorXd q)
55 {
56  Eigen::Matrix4d Q_(4, 4);
57 
58  Q_ << q(0), -q(1), -q(2), -q(3), q(1), q(0), -q(3), q(2), q(2), q(3), q(0),
59  -q(1), q(3), -q(2), q(1), q(0);
60 
61  return Q_;
62 }
63 
64 Eigen::Matrix4d lhm::qMatW(Eigen::VectorXd q)
65 {
66  Eigen::Matrix4d Q_(4, 4);
67 
68  Q_ << q(0), -q(1), -q(2), -q(3), q(1), q(0), q(3), -q(2), q(2), -q(3), q(0),
69  q(1), q(3), q(2), -q(1), q(0);
70 
71  return Q_;
72 }
73 
75 {
76  for (int i = 0; i < n; i++) Q.col(i) = F[i] * Q.col(i);
77 
78  Eigen::Vector3d P_bar, Q_bar;
79  P_bar = P.rowwise().mean();
80  Q_bar = Q.rowwise().mean();
81 
82  for (int i = 0; i < n; i++)
83  {
84  P.col(i) = P.col(i) - P_bar;
85  Q.col(i) = Q.col(i) - Q_bar;
86  }
87 
88  //<------------------- Use SVD Solution ------------------->//
89  /*
90  Eigen::Matrix3d M;
91  M.setZero();
92 
93  for (i = 0; i < n; i++)
94  M += P.col(i)*Q.col(i).transpose();
95 
96  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU |
97  Eigen::ComputeThinV);
98 
99  R = svd.matrixV()*svd.matrixU().transpose();
100 
101  Eigen::Matrix3d dummy;
102  dummy.col(0) = -svd.matrixV().col(0);
103  dummy.col(1) = -svd.matrixV().col(1);
104  dummy.col(2) = svd.matrixV().col(2);
105 
106  if (R.determinant() == 1)
107  {
108  estimate_t();
109  if (t(2) < 0)
110  {
111  R = dummy*svd.matrixU().transpose();
112  estimate_t();
113  }
114  }
115  else
116  {
117  R = -dummy*svd.matrixU().transpose();
118  estimate_t();
119  if (t(2) < 0)
120  {
121  R = -svd.matrixV()*svd.matrixU().transpose();
122  estimate_t();
123  }
124  }
125 
126  err2 = 0;
127  xform();
128 
129  Eigen::Vector3d vec;
130  Eigen::Matrix3d I3 = Eigen::MatrixXd::Identity(3, 3);
131 
132  for (i = 0; i < n; i++)
133  {
134  vec = (I3 - F[i])*Q.col(i);
135  err2 += vec.squaredNorm();
136  }
137  */
138  //<------------------- Use QTN Solution ------------------->//
139 
140  Eigen::Matrix4d A;
141  A.setZero();
142 
143  for (int i = 0; i < n; i++)
144  {
145  Eigen::Vector4d q1, q2;
146  q1 << 1, Q.col(i);
147  q2 << 1, P.col(i);
148  A += qMatQ(q1).transpose() * qMatW(q2);
149  }
150 
151  Eigen::EigenSolver<Eigen::Matrix4d> es(A);
152 
153  const Eigen::Matrix4d Ae = es.pseudoEigenvalueMatrix();
154  Eigen::Vector4d D; // Ae.diagonal(); for some reason this leads to an
155  // internal compiler error in MSVC11... (sigh)
156  for (int i = 0; i < 4; i++) D[i] = Ae(i, i);
157 
158  Eigen::Matrix4d V_mat = es.pseudoEigenvectors();
159 
160  Eigen::Vector4d::Index max_index;
161 
162  D.maxCoeff(&max_index);
163 
164  Eigen::Vector4d V;
165 
166  V = V_mat.col(max_index);
167 
168  Eigen::Quaterniond q(V(0), V(1), V(2), V(3));
169 
170  R = q.toRotationMatrix();
171 
172  estimate_t();
173 
174  err2 = 0;
175  xform();
176 
177  Eigen::Vector3d vec;
178  Eigen::Matrix3d I3 = Eigen::MatrixXd::Identity(3, 3);
179 
180  for (int i = 0; i < n; i++)
181  {
182  vec = (I3 - F[i]) * Q.col(i);
183  err2 += vec.squaredNorm();
184  }
185 }
186 
188  Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
189 {
190  int i, j = 0;
191 
192  Eigen::VectorXd p_bar;
193  Eigen::Matrix3d sum_F, I3;
194  I3 = Eigen::MatrixXd::Identity(3, 3);
195  sum_F.setZero();
196 
197  p_bar = P.rowwise().mean();
198 
199  for (i = 0; i < n; i++)
200  {
201  P.col(i) -= p_bar;
202  F[i] = Q.col(i) * Q.col(i).transpose() / Q.col(i).squaredNorm();
203  sum_F = sum_F + F[i];
204  }
205 
206  G = (I3 - sum_F / n).inverse() / n;
207 
208  err = 0;
209  err2 = 1000;
210  absKernel();
211 
212  while (std::abs(err2 - err) > TOL_LHM && err2 > EPSILON_LHM)
213  {
214  err = err2;
215 
216  absKernel();
217 
218  j += 1;
219  if (j > 100) break;
220  }
221 
222  R_ = R;
223  t_ = t - R * p_bar;
224 
225  return true;
226 }
Eigen::MatrixXd img_pts
Object points in Camera Co-ordinate system.
Definition: lhm.h:42
void xform()
Transform object points in Body frame (Landmark Frame) to estimated Camera Frame. ...
Definition: lhm.cpp:49
Eigen::MatrixXd P
Camera intrinsic matrix.
Definition: lhm.h:44
double err2
Error variable for convergence selection.
Definition: lhm.h:53
GLdouble GLdouble t
Definition: glext.h:3695
Eigen::MatrixXd obj_pts
Definition: lhm.h:41
void estimate_t()
Function to estimate translation given an estimated rotation matrix.
Definition: lhm.cpp:41
Eigen::Matrix4d qMatW(Eigen::VectorXd q)
Iternal Function of quaternion.
Definition: lhm.cpp:64
Eigen::Matrix3d G
Matrix for internal computations.
Definition: lhm.h:48
Perspective n Point (PnP) Algorithms toolkit for MRPT mrpt_vision_grp.
Definition: pnp_algos.h:23
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
int n
Error variable for convergence selection.
Definition: lhm.h:55
#define TOL_LHM
Definition: lhm.h:22
void absKernel()
Function to compute pose during an iteration.
Definition: lhm.cpp:74
double err
Storage matrix for each point.
Definition: lhm.h:52
Eigen::MatrixXd Q
Trnaspose of Object points .
Definition: lhm.h:45
Lu Hage Mjolsness - Iterative PnP Algorithm (Eigen Implementation.
std::vector< Eigen::Matrix3d > F
Translation Vector.
Definition: lhm.h:51
lhm(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0)
Number of 2d/3d correspondences.
Definition: lhm.cpp:22
#define EPSILON_LHM
Definition: lhm.h:23
Eigen::Matrix3d R
Transpose of Image points .
Definition: lhm.h:47
Eigen::Vector3d t
Rotation Matrix.
Definition: lhm.h:49
Eigen::MatrixXd cam_intrinsic
Image points in pixel co-ordinates.
Definition: lhm.h:43
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
Eigen::Matrix4d qMatQ(Eigen::VectorXd q)
Iternal Function of quaternion.
Definition: lhm.cpp:54



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019