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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020