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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 0cbd40372 Sun Nov 17 09:43:05 2019 +0100 at dom nov 17 09:45:09 CET 2019