Main MRPT website > C++ reference for MRPT 1.5.5
rpnp.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 #include <vector>
13 #include <cmath>
14 
15 #include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
16 #include <Eigen/Dense>
17 #include <Eigen/SVD>
18 #include <Eigen/StdVector>
19 #include <unsupported/Eigen/Polynomials>
20 
21 #include "rpnp.h"
22 
23 
24 mrpt::vision::pnp::rpnp::rpnp(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0)
25 {
26  obj_pts = obj_pts_;
27  img_pts = img_pts_;
28  cam_intrinsic = cam_;
29  n = n0;
30 
31  // Store obj_pts as 3XN and img_projections as 2XN matrices
32  P = obj_pts.transpose();
33  Q = img_pts.transpose();
34 
35  for (int i = 0; i < n; i++)
36  Q.col(i) = Q.col(i) / Q.col(i).norm();
37 
38  R.setZero();
39  t.setZero();
40 }
41 
42 bool mrpt::vision::pnp::rpnp::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
43 {
44  // selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling
45  int i1 = 0, i2 = 1;
46  double lmin = Q(0, i1)*Q(0, i2) + Q(1, i1)*Q(1, i2) + Q(2, i1)*Q(2, i2);
47 
48  Eigen::MatrixXi rij (n,2);
49 
50  R_=Eigen::MatrixXd::Identity(3,3);
51  t_=Eigen::Vector3d::Zero();
52 
53  for (int i = 0; i < n; i++)
54  for (int j = 0; j < 2; j++)
55  rij(i, j) = rand() % n;
56 
57  for (int ii = 0; ii < n; ii++)
58  {
59  int i = rij(ii, 0), j = rij(ii,1);
60 
61  if (i == j)
62  continue;
63 
64  double l = Q(0, i)*Q(0, j) + Q(1, i)*Q(1, j) + Q(2, i)*Q(2, j);
65 
66  if (l < lmin)
67  {
68  i1 = i;
69  i2 = j;
70  lmin = l;
71  }
72  }
73 
74  // calculating the rotation matrix of $O_aX_aY_aZ_a$.
75  Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec;
76 
77  p1 = P.col(i1);
78  p2 = P.col(i2);
79  p0 = (p1 + p2) / 2;
80 
81  x = p2 - p0; x /= x.norm();
82 
83  if (std::abs(x(1)) < std::abs(x(2)) )
84  {
85  dum_vec << 0, 1, 0;
86  z = x.cross(dum_vec); z /= z.norm();
87  y = z.cross(x); y /= y.norm();
88  }
89  else
90  {
91  dum_vec << 0, 0, 1;
92  y = dum_vec.cross(x); y /= y.norm();
93  z = x.cross(y); x /= x.norm();
94  }
95 
96  Eigen::Matrix3d R0;
97 
98  R0.col(0) = x; R0.col(1) =y; R0.col(2) = z;
99 
100  for (int i = 0; i < n; i++)
101  P.col(i) = R0.transpose() * (P.col(i) - p0);
102 
103  // Dividing the n - point set into(n - 2) 3 - point subsets
104  // and setting up the P3P equations
105 
106  Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2);
107  double cg1 = v1.dot(v2);
108  double sg1 = sqrt(1 - cg1*cg1);
109  double D1 = (P.col(i1) - P.col(i2)).norm();
110  Eigen::MatrixXd D4(n - 2, 5);
111 
112  int j = 0;
113  Eigen::Vector3d vi;
114  Eigen::VectorXd rowvec(5);
115  for (int i = 0; i < n; i++)
116  {
117  if (i == i1 || i == i2)
118  continue;
119 
120  vi = Q.col(i);
121  double cg2 = v1.dot(vi);
122  double cg3 = v2.dot(vi);
123  double sg2 = sqrt(1 - cg2*cg2);
124  double D2 = (P.col(i1) - P.col(i)).norm();
125  double D3 = (P.col(i) - P.col(i2)).norm();
126 
127  // get the coefficients of the P3P equation from each subset.
128 
129  rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3);
130  D4.row(j) = rowvec;
131  j += 1;
132 
133  if(j>n-3)
134  break;
135  }
136 
137  Eigen::VectorXd D7(8), dumvec(8), dumvec1(5);
138  D7.setZero();
139 
140  for (int i = 0; i < n-2; i++)
141  {
142  dumvec1 = D4.row(i);
143  dumvec= getpoly7(dumvec1);
144  D7 += dumvec;
145  }
146 
147  Eigen::PolynomialSolver<double, 7> psolve(D7.reverse());
148  Eigen::VectorXcd comp_roots = psolve.roots().transpose();
149  Eigen::VectorXd real_comp, imag_comp;
150  real_comp = comp_roots.real();
151  imag_comp = comp_roots.imag();
152 
153  Eigen::VectorXd::Index max_index;
154 
155  double max_real= real_comp.cwiseAbs().maxCoeff(&max_index);
156 
157  std::vector<double> act_roots_;
158 
159  int cnt=0;
160 
161  for (int i=0; i<imag_comp.size(); i++ )
162  {
163  if(std::abs(imag_comp(i))/max_real<0.001)
164  {
165  act_roots_.push_back(real_comp(i));
166  cnt++;
167  }
168  }
169 
170  double* ptr = &act_roots_[0];
171  Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt);
172 
173  if (cnt==0)
174  {
175  return false;
176  }
177 
178  Eigen::VectorXd act_roots1(cnt);
179  act_roots1 << act_roots.segment(0,cnt);
180 
181  std::vector<Eigen::Matrix3d> R_cum(cnt);
182  std::vector<Eigen::Vector3d> t_cum(cnt);
183  std::vector<double> err_cum(cnt);
184 
185  for(int i=0; i<cnt; i++)
186  {
187  double root = act_roots(i);
188 
189  // Compute the rotation matrix
190 
191  double d2 = cg1 + root;
192 
193  Eigen::Vector3d unitx, unity, unitz;
194  unitx << 1,0,0;
195  unity << 0,1,0;
196  unitz << 0,0,1;
197  x = v2*d2 -v1;
198  x/=x.norm();
199  if (std::abs(unity.dot(x)) < std::abs(unitz.dot(x)))
200  {
201  z = x.cross(unity);z/=z.norm();
202  y=z.cross(x); y/y.norm();
203  }
204  else
205  {
206  y=unitz.cross(x); y/=y.norm();
207  z = x.cross(y); z/=z.norm();
208  }
209  R.col(0)=x;
210  R.col(1)=y;
211  R.col(2)=z;
212 
213  //calculating c, s, tx, ty, tz
214 
215  Eigen::MatrixXd D(2 * n, 6);
216  D.setZero();
217 
218  R0 = R.transpose();
219  Eigen::VectorXd r(Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols()*R0.rows()));
220 
221  for (int j = 0; j<n; j++)
222  {
223  double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j), yi = P(1, j), zi = P(2, j);
224  D.row(2 * j) << -r(1)*yi + ui*(r(7)*yi + r(8)*zi) - r(2)*zi,
225  -r(2)*yi + ui*(r(8)*yi - r(7)*zi) + r(1)*zi,
226  -1,
227  0,
228  ui,
229  ui*r(6)*xi - r(0)*xi;
230 
231  D.row(2 * j + 1) << -r(4)*yi + vi*(r(7)*yi + r(8)*zi) - r(5)*zi,
232  -r(5)*yi + vi*(r(8)*yi - r(7)*zi) + r(4)*zi,
233  0,
234  -1,
235  vi,
236  vi*r(6)*xi - r(3)*xi;
237  }
238 
239  Eigen::MatrixXd DTD = D.transpose()*D;
240 
241  Eigen::EigenSolver<Eigen::MatrixXd> es(DTD);
242 
243  Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal();
244 
245  Eigen::MatrixXd V_mat = es.pseudoEigenvectors();
246 
247  Eigen::MatrixXd::Index min_index;
248 
249  Diag.minCoeff(&min_index);
250 
251  Eigen::VectorXd V = V_mat.col(min_index);
252 
253  V /= V(5);
254 
255  double c = V(0), s = V(1);
256  t << V(2), V(3), V(4);
257 
258  //calculating the camera pose by 3d alignment
259  Eigen::VectorXd xi, yi, zi;
260  xi = P.row(0);
261  yi = P.row(1);
262  zi = P.row(2);
263 
264  Eigen::MatrixXd XXcs(3, n), XXc(3,n);
265  XXc.setZero();
266 
267  XXcs.row(0) = r(0)*xi + (r(1)*c + r(2)*s)*yi + (-r(1)*s + r(2)*c)*zi + t(0)*Eigen::VectorXd::Ones(n);
268  XXcs.row(1) = r(3)*xi + (r(4)*c + r(5)*s)*yi + (-r(4)*s + r(5)*c)*zi + t(1)*Eigen::VectorXd::Ones(n);
269  XXcs.row(2) = r(6)*xi + (r(7)*c + r(8)*s)*yi + (-r(7)*s + r(8)*c)*zi + t(2)*Eigen::VectorXd::Ones(n);
270 
271  for (int ii = 0; ii < n; ii++)
272  XXc.col(ii) = Q.col(ii)*XXcs.col(ii).norm();
273 
274  Eigen::Matrix3d R2;
275  Eigen::Vector3d t2;
276 
277  Eigen::MatrixXd XXw = obj_pts.transpose();
278 
279  calcampose(XXc, XXw, R2, t2);
280 
281  R_cum[i] = R2;
282  t_cum[i] = t2;
283 
284  for (int k = 0; k < n; k++)
285  XXc.col(k) = R2 * XXw.col(k) + t2;
286 
287  Eigen::MatrixXd xxc(2, n);
288 
289  xxc.row(0) = XXc.row(0).array() / XXc.row(2).array();
290  xxc.row(1) = XXc.row(1).array() / XXc.row(2).array();
291 
292  double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() + (xxc.row(1) - img_pts.col(1).transpose()).norm()) / 2;
293 
294  err_cum[i] = res;
295 
296  }
297 
298  int pos_cum = std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin();
299 
300  R_ = R_cum[pos_cum];
301  t_ = t_cum[pos_cum];
302 
303  return true;
304 }
305 
306 void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
307 {
308  Eigen::MatrixXd X = XXc;
309  Eigen::MatrixXd Y = XXw;
310  Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
311  Eigen::VectorXd ux, uy;
312  uy = X.rowwise().mean();
313  ux = Y.rowwise().mean();
314 
315  // Need to verify sigmax2
316  double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();
317 
318  Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;
319 
320  Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);
321 
322  Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
323  if (SXY.determinant() <0)
324  S(2, 2) = -1;
325 
326  R2 = svd.matrixV()*S*svd.matrixU().transpose();
327 
328  double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
329  t2 = uy - c2*R2*ux;
330 
331  Eigen::Vector3d x, y, z;
332  x = R2.col(0);
333  y = R2.col(1);
334  z = R2.col(2);
335 
336  if ((x.cross(y) - z).norm()>0.02)
337  R2.col(2) = -R2.col(2);
338 }
339 
340 Eigen::VectorXd mrpt::vision::pnp::rpnp::getpoly7(const Eigen::VectorXd& vin)
341 {
342  Eigen::VectorXd vout(8);
343  vout << 4 * pow(vin(0), 2),
344  7 * vin(1) * vin(0),
345  6 * vin(2)*vin(0) + 3 * pow(vin(1), 2),
346  5 * vin(3)*vin(0) + 5 * vin(2)*vin(1),
347  4 * vin(4)*vin(0) + 4 * vin(3)*vin(1) + 2 * pow(vin(2), 2),
348  3 * vin(4)*vin(1) + 3 * vin(3)*vin(2),
349  2 * vin(4)*vin(2) + pow(vin(3), 2),
350  vin(4)*vin(3);
351  return vout;
352 }
353 
354 Eigen::VectorXd mrpt::vision::pnp::rpnp::getp3p(double l1, double l2, double A5, double C1, double C2, double D1, double D2, double D3)
355 {
356  double A1 = (D2 / D1)*(D2 / D1);
357  double A2 = A1*pow(C1, 2) - pow(C2, 2);
358  double A3 = l2*A5 - l1;
359  double A4 = l1*A5 - l2;
360  double A6 = (pow(D3, 2) - pow(D1, 2) - pow(D2, 2)) / (2 * pow(D1, 2));
361  double A7 = 1 - pow(l1, 2) - pow(l2, 2) + l1*l2*A5 + A6*pow(C1, 2);
362 
363  Eigen::VectorXd vec(5);
364 
365  vec << pow(A6, 2) - A1*pow(A5, 2),
366  2 * (A3*A6 - A1*A4*A5),
367  pow(A3, 2) + 2 * A6*A7 - A1*pow(A4, 2) - A2*pow(A5, 2),
368  2 * (A3*A7 - A2*A4*A5),
369  pow(A7, 2) - A2*pow(A4, 2);
370 
371  return vec;
372 }
Eigen::MatrixXd P
Camera Intrinsic Matrix.
Definition: rpnp.h:28
void calcampose(Eigen::MatrixXd &XXc, Eigen::MatrixXd &XXw, Eigen::Matrix3d &R2, Eigen::Vector3d &t2)
Function to calculate final pose.
Definition: rpnp.cpp:306
GLdouble GLdouble t
Definition: glext.h:3610
GLdouble GLdouble z
Definition: glext.h:3734
Eigen::MatrixXd cam_intrinsic
Image Points (n X 3) in Camera Co-ordinate system.
Definition: rpnp.h:27
rpnp(Eigen::MatrixXd obj_pts_, Eigen::MatrixXd img_pts_, Eigen::MatrixXd cam_, int n0)
Number of 2D/3D correspondences.
Definition: rpnp.cpp:24
GLenum GLsizei n
Definition: glext.h:4618
Eigen::VectorXd getp3p(double l1, double l2, double A5, double C1, double C2, double D1, double D2, double D3)
Function to compute pose using P3P.
Definition: rpnp.cpp:354
GLdouble s
Definition: glext.h:3602
Eigen::MatrixXd img_pts
Object Points (n X 3) in Camera Co-ordinate system.
Definition: rpnp.h:26
Robust - PnP class definition for computing pose.
const GLubyte * c
Definition: glext.h:5590
bool compute_pose(Eigen::Ref< Eigen::Matrix3d > R_, Eigen::Ref< Eigen::Vector3d > t_)
Function to compute pose.
Definition: rpnp.cpp:42
double A1
UTC constant and 1st order terms.
int n
Translation vector.
Definition: rpnp.h:33
Eigen::MatrixXd obj_pts
Definition: rpnp.h:25
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
GLfloat GLfloat v1
Definition: glext.h:3922
const float R
Eigen::MatrixXd Q
Transposed Object Points (3 X n) for computations.
Definition: rpnp.h:29
Eigen::Matrix3d R
Transposed Image Points (3 X n) for computations.
Definition: rpnp.h:31
Eigen::VectorXd getpoly7(const Eigen::VectorXd &vin)
Get Polynomial from input vector.
Definition: rpnp.cpp:340
GLenum GLint GLint y
Definition: glext.h:3516
GLfloat GLfloat GLfloat v2
Definition: glext.h:3923
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
CONTAINER::Scalar norm(const CONTAINER &v)



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