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



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST