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



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