Main MRPT website > C++ reference for MRPT 1.5.6
dls.h
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 #ifndef DLS_H_
11 #define DLS_H_
12 
13 #include <mrpt/otherlibs/do_opencv_includes.h>
14 
15 #if MRPT_HAS_OPENCV
16 
17  namespace mrpt
18  {
19  namespace vision
20  {
21  namespace pnp
22  {
23  /** \addtogroup pnp Perspective-n-Point pose estimation
24  * \ingroup mrpt_vision_grp
25  * @{
26  */
27 
28  /**
29  * @class dls
30  * @author Chandra Mangipudi
31  * @date 12/08/16
32  * @file dls.h
33  * @brief Direct Least Squares (DLS) - Eigen wrapper for OpenCV Implementation
34  */
35  class dls
36  {
37  public:
38  //! Constructor for DLS class
39  dls(const cv::Mat& opoints, const cv::Mat& ipoints);
40  ~dls();
41 
42  //! OpenCV function for computing pose
43  bool compute_pose(cv::Mat& R, cv::Mat& t);
44 
45  private:
46 
47  /**
48  * @brief Initialization of object points and image points
49  * @param[in] opoints
50  * @param[in] ipoints
51  */
52  template <typename OpointType, typename IpointType>
53  void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
54  {
55  for(int i = 0; i < N; i++)
56  {
57  p.at<double>(0,i) = opoints.at<OpointType>(i).x;
58  p.at<double>(1,i) = opoints.at<OpointType>(i).y;
59  p.at<double>(2,i) = opoints.at<OpointType>(i).z;
60 
61  // compute mean of object points
62  mn.at<double>(0) += p.at<double>(0,i);
63  mn.at<double>(1) += p.at<double>(1,i);
64  mn.at<double>(2) += p.at<double>(2,i);
65 
66  // make z into unit vectors from normalized pixel coords
67  double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
68  std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
69  sr = std::sqrt(sr);
70 
71  z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
72  z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
73  z.at<double>(2,i) = (double)1 / sr;
74  }
75 
76  mn.at<double>(0) /= (double)N;
77  mn.at<double>(1) /= (double)N;
78  mn.at<double>(2) /= (double)N;
79  }
80 
81  /**
82  * @brief Create a matrix from vector
83  * @param[in] v
84  * @return Matrix containing vector v as columns
85  */
86  cv::Mat LeftMultVec(const cv::Mat& v);
87 
88  /**
89  * @brief Main function to run DLS-PnP
90  * @param[in] pp
91  */
92  void run_kernel(const cv::Mat& pp);
93 
94  /**
95  * @brief Build the Maucaulay matrix co-efficients
96  * @param[in] pp
97  * @param[out] Mtilde
98  * @param[out] D
99  */
100  void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
101 
102  /**
103  * @brief Eigen Value Decomposition
104  * @param Mtilde Matrix to be decomposed
105  * @param eigenval_real Real eigenvalues
106  * @param eigenval_imag Imaginary eigenvalues
107  * @param eigenvec_real Eigen Vectors corresponding to real eigen values
108  * @param eigenvec_imag Eigen Vectors corresponding to imaginary eigen values
109  */
110  void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
111  cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
112 
113  /**
114  * @brief Fill the hessian functions
115  * @param[in] D
116  */
117  void fill_coeff(const cv::Mat * D);
118 
119  /**
120  * @brief Fill the Maucaulay matrix with co-efficients
121  * @param[in] a
122  * @param[in] b
123  * @param[in] c
124  * @param[in] u
125  * @return Maucaulay matrix M
126  */
127  cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
128  const std::vector<double>& c, const std::vector<double>& u);
129 
130  /**
131  * @brief Compute the Hessian matrix for the polynomial co-efficient vector s
132  * @param[in] s
133  * @return
134  */
135  cv::Mat Hessian(const double s[]);
136 
137  /**
138  * @brief Cayley parameters to Rotation Matrix
139  * @param[in] s
140  * @return Rotation Matrix
141  */
142  cv::Mat cayley2rotbar(const cv::Mat& s);
143 
144  /**
145  * @brief Create a skwy-symmetric matrix from a vector
146  * @param[in] X1
147  * @return Skew-symmetric matrix
148  */
149  cv::Mat skewsymm(const cv::Mat * X1);
150 
151  /**
152  * @brief Rotation matrix along x-axis by angle t
153  * @param[in] t
154  * @return Rotation Matrix
155  */
156  cv::Mat rotx(const double t);
157 
158  /**
159  * @brief Rotation matrix along y-axis by angle t
160  * @param[in] t
161  * @return Rotation Matrix
162  */
163  cv::Mat roty(const double t);
164 
165  /**
166  * @brief Rotation matrix along z-axis by angle t
167  * @param[in] t
168  * @return Rotation Matrix
169  */
170  cv::Mat rotz(const double t);
171 
172  /**
173  * @brief Column-wise mean of matrix M
174  * @param[in] M
175  * @return Mean vector
176  */
177  cv::Mat mean(const cv::Mat& M);
178 
179  /**
180  * @brief Check for negative values in vector v
181  * @param[in] v
182  * @return False if v[i] < 0 else True
183  */
184  bool is_empty(const cv::Mat * v);
185 
186  /**
187  * @brief check for positive eigenvalues
188  * @param[in] eigenvalues
189  * @return True if positivie eigenvalues are found else False
190  */
191  bool positive_eigenvalues(const cv::Mat * eigenvalues);
192 
193  cv::Mat p, z, mn; //! object-image points
194  int N; //! number of input points
195  std::vector<double> f1coeff, f2coeff, f3coeff, cost_; //! coefficient for coefficients matrix
196  std::vector<cv::Mat> C_est_, t_est_; //! optimal candidates
197  cv::Mat C_est__, t_est__; //! optimal found solution
198  double cost__; //! cost for optimal found solution
199  };
200 
201 
202  /**
203  * \cond INTERNAL_FUNC_DLS
204  */
205  class EigenvalueDecomposition {
206  private:
207 
208 
209  int n; //! Holds the data dimension.
210 
211  double cdivr, cdivi; //! Stores real/imag part of a complex division.
212 
213 
214  double *d, *e, *ort; //! Pointer to internal memory.
215  double **V, **H; //! Pointer to internal memory.
216 
217  cv::Mat _eigenvalues; //! Holds the computed eigenvalues.
218 
219  cv::Mat _eigenvectors; //! Holds the computed eigenvectors.
220 
221  /**
222  * @brief Function to allocate memmory for 1d array
223  * @param[in] m Size of new 1d array
224  * @return New 1d array of appropriate type
225  */
226  template<typename _Tp>
227  _Tp *alloc_1d(int m) {
228  return new _Tp[m];
229  }
230 
231  /**
232  * @brief Function to allocate memmory and initialize 1d array
233  * @param[in] m Size of new 1d array
234  * @param[in] val Initial values for the array
235  * @return New 1d array
236  */
237  template<typename _Tp>
238  _Tp *alloc_1d(int m, _Tp val) {
239  _Tp *arr = alloc_1d<_Tp> (m);
240  for (int i = 0; i < m; i++)
241  arr[i] = val;
242  return arr;
243  }
244 
245  /**
246  * @brief Function to allocate memmory for 2d array
247  * @param m Row size
248  * @param _n Column size
249  * @return New 2d array
250  */
251  template<typename _Tp>
252  _Tp **alloc_2d(int m, int _n) {
253  _Tp **arr = new _Tp*[m];
254  for (int i = 0; i < m; i++)
255  arr[i] = new _Tp[_n];
256  return arr;
257  }
258 
259  /**
260  * @brief Function to allocate memmory for 2d array and initialize the array
261  * @param[in] m Row size
262  * @param[in] _n Column size
263  * @param[out] val Initialization for 2d array
264  * @return
265  */
266  template<typename _Tp>
267  _Tp **alloc_2d(int m, int _n, _Tp val) {
268  _Tp **arr = alloc_2d<_Tp> (m, _n);
269  for (int i = 0; i < m; i++) {
270  for (int j = 0; j < _n; j++) {
271  arr[i][j] = val;
272  }
273  }
274  return arr;
275  }
276 
277  /**
278  * @brief Internal function
279  * @param[in] xr
280  * @param[in] xi
281  * @param[in] yr
282  * @param[in] yi
283  */
284  void cdiv(double xr, double xi, double yr, double yi) {
285  double r, dv;
286  if (std::abs(yr) > std::abs(yi)) {
287  r = yi / yr;
288  dv = yr + r * yi;
289  cdivr = (xr + r * xi) / dv;
290  cdivi = (xi - r * xr) / dv;
291  } else {
292  r = yr / yi;
293  dv = yi + r * yr;
294  cdivr = (r * xr + xi) / dv;
295  cdivi = (r * xi - xr) / dv;
296  }
297  }
298 
299  /**
300  * @brief Nonsymmetric reduction from Hessenberg to real Schur form.
301  *
302  * This is derived from the Algol procedure hqr2,
303  * by Martin and Wilkinson, Handbook for Auto. Comp.,
304  * Vol.ii-Linear Algebra, and the corresponding
305  * Fortran subroutine in EISPACK.
306  */
307  void hqr2() {
308  // Initialize
309  int nn = this->n;
310  int n1 = nn - 1;
311  int low = 0;
312  int high = nn - 1;
313  double eps = std::pow(2.0, -52.0);
314  double exshift = 0.0;
315  double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;
316 
317  // Store roots isolated by balanc and compute matrix norm
318 
319  double norm = 0.0;
320  for (int i = 0; i < nn; i++) {
321  if (i < low || i > high) {
322  d[i] = H[i][i];
323  e[i] = 0.0;
324  }
325  for (int j = std::max(i - 1, 0); j < nn; j++) {
326  norm = norm + std::abs(H[i][j]);
327  }
328  }
329 
330  // Outer loop over eigenvalue index
331  int iter = 0;
332  while (n1 >= low) {
333 
334  // Look for single small sub-diagonal element
335  int l = n1;
336  while (l > low) {
337  s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
338  if (s == 0.0) {
339  s = norm;
340  }
341  if (std::abs(H[l][l - 1]) < eps * s) {
342  break;
343  }
344  l--;
345  }
346 
347  // Check for convergence
348  // One root found
349 
350  if (l == n1) {
351  H[n1][n1] = H[n1][n1] + exshift;
352  d[n1] = H[n1][n1];
353  e[n1] = 0.0;
354  n1--;
355  iter = 0;
356 
357  // Two roots found
358 
359  } else if (l == n1 - 1) {
360  w = H[n1][n1 - 1] * H[n1 - 1][n1];
361  p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0;
362  q = p * p + w;
363  z = std::sqrt(std::abs(q));
364  H[n1][n1] = H[n1][n1] + exshift;
365  H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift;
366  x = H[n1][n1];
367 
368  // Real pair
369 
370  if (q >= 0) {
371  if (p >= 0) {
372  z = p + z;
373  } else {
374  z = p - z;
375  }
376  d[n1 - 1] = x + z;
377  d[n1] = d[n1 - 1];
378  if (z != 0.0) {
379  d[n1] = x - w / z;
380  }
381  e[n1 - 1] = 0.0;
382  e[n1] = 0.0;
383  x = H[n1][n1 - 1];
384  s = std::abs(x) + std::abs(z);
385  p = x / s;
386  q = z / s;
387  r = std::sqrt(p * p + q * q);
388  p = p / r;
389  q = q / r;
390 
391  // Row modification
392 
393  for (int j = n1 - 1; j < nn; j++) {
394  z = H[n1 - 1][j];
395  H[n1 - 1][j] = q * z + p * H[n1][j];
396  H[n1][j] = q * H[n1][j] - p * z;
397  }
398 
399  // Column modification
400 
401  for (int i = 0; i <= n1; i++) {
402  z = H[i][n1 - 1];
403  H[i][n1 - 1] = q * z + p * H[i][n1];
404  H[i][n1] = q * H[i][n1] - p * z;
405  }
406 
407  // Accumulate transformations
408 
409  for (int i = low; i <= high; i++) {
410  z = V[i][n1 - 1];
411  V[i][n1 - 1] = q * z + p * V[i][n1];
412  V[i][n1] = q * V[i][n1] - p * z;
413  }
414 
415  // Complex pair
416 
417  } else {
418  d[n1 - 1] = x + p;
419  d[n1] = x + p;
420  e[n1 - 1] = z;
421  e[n1] = -z;
422  }
423  n1 = n1 - 2;
424  iter = 0;
425 
426  // No convergence yet
427 
428  } else {
429 
430  // Form shift
431 
432  x = H[n1][n1];
433  y = 0.0;
434  w = 0.0;
435  if (l < n1) {
436  y = H[n1 - 1][n1 - 1];
437  w = H[n1][n1 - 1] * H[n1 - 1][n1];
438  }
439 
440  // Wilkinson's original ad hoc shift
441 
442  if (iter == 10) {
443  exshift += x;
444  for (int i = low; i <= n1; i++) {
445  H[i][i] -= x;
446  }
447  s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]);
448  x = y = 0.75 * s;
449  w = -0.4375 * s * s;
450  }
451 
452  // MATLAB's new ad hoc shift
453 
454  if (iter == 30) {
455  s = (y - x) / 2.0;
456  s = s * s + w;
457  if (s > 0) {
458  s = std::sqrt(s);
459  if (y < x) {
460  s = -s;
461  }
462  s = x - w / ((y - x) / 2.0 + s);
463  for (int i = low; i <= n1; i++) {
464  H[i][i] -= s;
465  }
466  exshift += s;
467  x = y = w = 0.964;
468  }
469  }
470 
471  iter = iter + 1; // (Could check iteration count here.)
472 
473  // Look for two consecutive small sub-diagonal elements
474  int m = n1 - 2;
475  while (m >= l) {
476  z = H[m][m];
477  r = x - z;
478  s = y - z;
479  p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
480  q = H[m + 1][m + 1] - z - r - s;
481  r = H[m + 2][m + 1];
482  s = std::abs(p) + std::abs(q) + std::abs(r);
483  p = p / s;
484  q = q / s;
485  r = r / s;
486  if (m == l) {
487  break;
488  }
489  if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p)
490  * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(
491  H[m + 1][m + 1])))) {
492  break;
493  }
494  m--;
495  }
496 
497  for (int i = m + 2; i <= n1; i++) {
498  H[i][i - 2] = 0.0;
499  if (i > m + 2) {
500  H[i][i - 3] = 0.0;
501  }
502  }
503 
504  // Double QR step involving rows l:n and columns m:n
505 
506  for (int k = m; k <= n1 - 1; k++) {
507  bool notlast = (k != n1 - 1);
508  if (k != m) {
509  p = H[k][k - 1];
510  q = H[k + 1][k - 1];
511  r = (notlast ? H[k + 2][k - 1] : 0.0);
512  x = std::abs(p) + std::abs(q) + std::abs(r);
513  if (x != 0.0) {
514  p = p / x;
515  q = q / x;
516  r = r / x;
517  }
518  }
519  if (x == 0.0) {
520  break;
521  }
522  s = std::sqrt(p * p + q * q + r * r);
523  if (p < 0) {
524  s = -s;
525  }
526  if (s != 0) {
527  if (k != m) {
528  H[k][k - 1] = -s * x;
529  } else if (l != m) {
530  H[k][k - 1] = -H[k][k - 1];
531  }
532  p = p + s;
533  x = p / s;
534  y = q / s;
535  z = r / s;
536  q = q / p;
537  r = r / p;
538 
539  // Row modification
540 
541  for (int j = k; j < nn; j++) {
542  p = H[k][j] + q * H[k + 1][j];
543  if (notlast) {
544  p = p + r * H[k + 2][j];
545  H[k + 2][j] = H[k + 2][j] - p * z;
546  }
547  H[k][j] = H[k][j] - p * x;
548  H[k + 1][j] = H[k + 1][j] - p * y;
549  }
550 
551  // Column modification
552 
553  for (int i = 0; i <= std::min(n1, k + 3); i++) {
554  p = x * H[i][k] + y * H[i][k + 1];
555  if (notlast) {
556  p = p + z * H[i][k + 2];
557  H[i][k + 2] = H[i][k + 2] - p * r;
558  }
559  H[i][k] = H[i][k] - p;
560  H[i][k + 1] = H[i][k + 1] - p * q;
561  }
562 
563  // Accumulate transformations
564 
565  for (int i = low; i <= high; i++) {
566  p = x * V[i][k] + y * V[i][k + 1];
567  if (notlast) {
568  p = p + z * V[i][k + 2];
569  V[i][k + 2] = V[i][k + 2] - p * r;
570  }
571  V[i][k] = V[i][k] - p;
572  V[i][k + 1] = V[i][k + 1] - p * q;
573  }
574  } // (s != 0)
575  } // k loop
576  } // check convergence
577  } // while (n1 >= low)
578 
579  // Backsubstitute to find vectors of upper triangular form
580 
581  if (norm == 0.0) {
582  return;
583  }
584 
585  for (n1 = nn - 1; n1 >= 0; n1--) {
586  p = d[n1];
587  q = e[n1];
588 
589  // Real vector
590 
591  if (q == 0) {
592  int l = n1;
593  H[n1][n1] = 1.0;
594  for (int i = n1 - 1; i >= 0; i--) {
595  w = H[i][i] - p;
596  r = 0.0;
597  for (int j = l; j <= n1; j++) {
598  r = r + H[i][j] * H[j][n1];
599  }
600  if (e[i] < 0.0) {
601  z = w;
602  s = r;
603  } else {
604  l = i;
605  if (e[i] == 0.0) {
606  if (w != 0.0) {
607  H[i][n1] = -r / w;
608  } else {
609  H[i][n1] = -r / (eps * norm);
610  }
611 
612  // Solve real equations
613 
614  } else {
615  x = H[i][i + 1];
616  y = H[i + 1][i];
617  q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
618  t = (x * s - z * r) / q;
619  H[i][n1] = t;
620  if (std::abs(x) > std::abs(z)) {
621  H[i + 1][n1] = (-r - w * t) / x;
622  } else {
623  H[i + 1][n1] = (-s - y * t) / z;
624  }
625  }
626 
627  // Overflow control
628 
629  t = std::abs(H[i][n1]);
630  if ((eps * t) * t > 1) {
631  for (int j = i; j <= n1; j++) {
632  H[j][n1] = H[j][n1] / t;
633  }
634  }
635  }
636  }
637  // Complex vector
638  } else if (q < 0) {
639  int l = n1 - 1;
640 
641  // Last vector component imaginary so matrix is triangular
642 
643  if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) {
644  H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1];
645  H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1];
646  } else {
647  cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q);
648  H[n1 - 1][n1 - 1] = cdivr;
649  H[n1 - 1][n1] = cdivi;
650  }
651  H[n1][n1 - 1] = 0.0;
652  H[n1][n1] = 1.0;
653  for (int i = n1 - 2; i >= 0; i--) {
654  double ra, sa;
655  ra = 0.0;
656  sa = 0.0;
657  for (int j = l; j <= n1; j++) {
658  ra = ra + H[i][j] * H[j][n1 - 1];
659  sa = sa + H[i][j] * H[j][n1];
660  }
661  w = H[i][i] - p;
662 
663  if (e[i] < 0.0) {
664  z = w;
665  r = ra;
666  s = sa;
667  } else {
668  l = i;
669  if (e[i] == 0) {
670  cdiv(-ra, -sa, w, q);
671  H[i][n1 - 1] = cdivr;
672  H[i][n1] = cdivi;
673  } else {
674 
675  // Solve complex equations
676 
677  x = H[i][i + 1];
678  y = H[i + 1][i];
679  double vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
680  double vi = (d[i] - p) * 2.0 * q;
681  if (vr == 0.0 && vi == 0.0) {
682  vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x)
683  + std::abs(y) + std::abs(z));
684  }
685  cdiv(x * r - z * ra + q * sa,
686  x * s - z * sa - q * ra, vr, vi);
687  H[i][n1 - 1] = cdivr;
688  H[i][n1] = cdivi;
689  if (std::abs(x) > (std::abs(z) + std::abs(q))) {
690  H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q
691  * H[i][n1]) / x;
692  H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1
693  - 1]) / x;
694  } else {
695  cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z,
696  q);
697  H[i + 1][n1 - 1] = cdivr;
698  H[i + 1][n1] = cdivi;
699  }
700  }
701 
702  // Overflow control
703 
704  t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1]));
705  if ((eps * t) * t > 1) {
706  for (int j = i; j <= n1; j++) {
707  H[j][n1 - 1] = H[j][n1 - 1] / t;
708  H[j][n1] = H[j][n1] / t;
709  }
710  }
711  }
712  }
713  }
714  }
715 
716  // Vectors of isolated roots
717 
718  for (int i = 0; i < nn; i++) {
719  if (i < low || i > high) {
720  for (int j = i; j < nn; j++) {
721  V[i][j] = H[i][j];
722  }
723  }
724  }
725 
726  // Back transformation to get eigenvectors of original matrix
727 
728  for (int j = nn - 1; j >= low; j--) {
729  for (int i = low; i <= high; i++) {
730  z = 0.0;
731  for (int k = low; k <= std::min(j, high); k++) {
732  z = z + V[i][k] * H[k][j];
733  }
734  V[i][j] = z;
735  }
736  }
737  }
738 
739  /**
740  * @brief Nonsymmetric reduction to Hessenberg form.
741  *
742  * This is derived from the Algol procedures orthes and ortran,
743  * by Martin and Wilkinson, Handbook for Auto. Comp.,
744  * Vol.ii-Linear Algebra, and the corresponding
745  * Fortran subroutines in EISPACK.
746  */
747  void orthes() {
748 
749  int low = 0;
750  int high = n - 1;
751 
752  for (int m = low + 1; m <= high - 1; m++) {
753 
754  // Scale column.
755 
756  double scale = 0.0;
757  for (int i = m; i <= high; i++) {
758  scale = scale + std::abs(H[i][m - 1]);
759  }
760  if (scale != 0.0) {
761 
762  // Compute Householder transformation.
763 
764  double h = 0.0;
765  for (int i = high; i >= m; i--) {
766  ort[i] = H[i][m - 1] / scale;
767  h += ort[i] * ort[i];
768  }
769  double g = std::sqrt(h);
770  if (ort[m] > 0) {
771  g = -g;
772  }
773  h = h - ort[m] * g;
774  ort[m] = ort[m] - g;
775 
776  // Apply Householder similarity transformation
777  // H = (I-u*u'/h)*H*(I-u*u')/h)
778 
779  for (int j = m; j < n; j++) {
780  double f = 0.0;
781  for (int i = high; i >= m; i--) {
782  f += ort[i] * H[i][j];
783  }
784  f = f / h;
785  for (int i = m; i <= high; i++) {
786  H[i][j] -= f * ort[i];
787  }
788  }
789 
790  for (int i = 0; i <= high; i++) {
791  double f = 0.0;
792  for (int j = high; j >= m; j--) {
793  f += ort[j] * H[i][j];
794  }
795  f = f / h;
796  for (int j = m; j <= high; j++) {
797  H[i][j] -= f * ort[j];
798  }
799  }
800  ort[m] = scale * ort[m];
801  H[m][m - 1] = scale * g;
802  }
803  }
804 
805  // Accumulate transformations (Algol's ortran).
806 
807  for (int i = 0; i < n; i++) {
808  for (int j = 0; j < n; j++) {
809  V[i][j] = (i == j ? 1.0 : 0.0);
810  }
811  }
812 
813  for (int m = high - 1; m >= low + 1; m--) {
814  if (H[m][m - 1] != 0.0) {
815  for (int i = m + 1; i <= high; i++) {
816  ort[i] = H[i][m - 1];
817  }
818  for (int j = m; j <= high; j++) {
819  double g = 0.0;
820  for (int i = m; i <= high; i++) {
821  g += ort[i] * V[i][j];
822  }
823  // Double division avoids possible underflow
824  g = (g / ort[m]) / H[m][m - 1];
825  for (int i = m; i <= high; i++) {
826  V[i][j] += g * ort[i];
827  }
828  }
829  }
830  }
831  }
832 
833  /**
834  * @brief Releases all internal working memory.
835  */
836  void release() {
837  // releases the working data
838  delete[] d;
839  delete[] e;
840  delete[] ort;
841  for (int i = 0; i < n; i++) {
842  delete[] H[i];
843  delete[] V[i];
844  }
845  delete[] H;
846  delete[] V;
847  }
848 
849  /**
850  * @brief Computes the Eigenvalue Decomposition for a matrix given in H.
851  */
852  void compute() {
853  // Allocate memory for the working data.
854  V = alloc_2d<double> (n, n, 0.0);
855  d = alloc_1d<double> (n);
856  e = alloc_1d<double> (n);
857  ort = alloc_1d<double> (n);
858  // Reduce to Hessenberg form.
859  orthes();
860  // Reduce Hessenberg to real Schur form.
861  hqr2();
862  // Copy eigenvalues to OpenCV Matrix.
863  _eigenvalues.create(1, n, CV_64FC1);
864  for (int i = 0; i < n; i++) {
865  _eigenvalues.at<double> (0, i) = d[i];
866  }
867  // Copy eigenvectors to OpenCV Matrix.
868  _eigenvectors.create(n, n, CV_64FC1);
869  for (int i = 0; i < n; i++)
870  for (int j = 0; j < n; j++)
871  _eigenvectors.at<double> (i, j) = V[i][j];
872  // Deallocate the memory by releasing all internal working data.
873  release();
874  }
875 
876 
877  public:
878  //! Constructor for EigenvalueDecomposition class
879  EigenvalueDecomposition()
880  : n(0) { }
881 
882  /**
883  * Initializes & computes the Eigenvalue Decomposition for a general matrix
884  * given in src. This function is a port of the EigenvalueSolver in JAMA,
885  * which has been released to public domain by The MathWorks and the
886  * National Institute of Standards and Technology (NIST).
887  */
888  EigenvalueDecomposition(cv::InputArray src) {
889  compute(src);
890  }
891 
892  /** This function computes the Eigenvalue Decomposition for a general matrix
893  * given in src. This function is a port of the EigenvalueSolver in JAMA,
894  * which has been released to public domain by The MathWorks and the
895  * National Institute of Standards and Technology (NIST).
896  */
897  void compute(cv::InputArray src)
898  {
899  /*if(isSymmetric(src)) {
900  // Fall back to OpenCV for a symmetric matrix!
901  cv::eigen(src, _eigenvalues, _eigenvectors);
902  } else {*/
903  cv::Mat tmp;
904  // Convert the given input matrix to double. Is there any way to
905  // prevent allocating the temporary memory? Only used for copying
906  // into working memory and deallocated after.
907  src.getMat().convertTo(tmp, CV_64FC1);
908  // Get dimension of the matrix.
909  this->n = tmp.cols;
910  // Allocate the matrix data to work on.
911  this->H = alloc_2d<double> (n, n);
912  // Now safely copy the data.
913  for (int i = 0; i < tmp.rows; i++) {
914  for (int j = 0; j < tmp.cols; j++) {
915  this->H[i][j] = tmp.at<double>(i, j);
916  }
917  }
918  // Deallocates the temporary matrix before computing.
919  tmp.release();
920  // Performs the eigenvalue decomposition of H.
921  compute();
922  // }
923  }
924 
925  //! Destructor for EigenvalueDecomposition class
926  ~EigenvalueDecomposition() {}
927 
928  /**
929  * @brief Returns the eigenvalues of the Eigenvalue Decomposition.
930  * @return eigenvalues
931  */
932  cv::Mat eigenvalues() { return _eigenvalues; }
933 
934  /**
935  * @brief Returns the eigenvectors of the Eigenvalue Decomposition.
936  * @return eigenvectors
937  */
938  cv::Mat eigenvectors() { return _eigenvectors; }
939  };
940 
941  /**
942  * \endcond
943  */
944 
945  /** @} */ // end of grouping
946 
947  }
948  }
949  }
950 #endif // OPENCV_Check
951 #endif // DLS_H
mrpt::vision::pnp::dls::is_empty
bool is_empty(const cv::Mat *v)
Check for negative values in vector v.
t
GLdouble GLdouble t
Definition: glext.h:3610
z
GLdouble GLdouble z
Definition: glext.h:3734
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
mrpt::vision::pnp::dls::compute_pose
bool compute_pose(cv::Mat &R, cv::Mat &t)
OpenCV function for computing pose.
mrpt::vision::pnp::dls::build_coeff_matrix
void build_coeff_matrix(const cv::Mat &pp, cv::Mat &Mtilde, cv::Mat &D)
Build the Maucaulay matrix co-efficients.
mrpt::vision::pnp::dls::t_est__
cv::Mat t_est__
Definition: dls.h:197
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
mrpt::vision::pnp::dls::fill_coeff
void fill_coeff(const cv::Mat *D)
Fill the hessian functions.
mrpt::vision::pnp::dls::f1coeff
std::vector< double > f1coeff
number of input points
Definition: dls.h:195
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
n
GLenum GLsizei n
Definition: glext.h:4618
mrpt::vision::pnp::dls::roty
cv::Mat roty(const double t)
Rotation matrix along y-axis by angle t.
mrpt::vision::pnp::dls::f2coeff
std::vector< double > f2coeff
Definition: dls.h:195
mrpt::vision::pnp::dls::N
int N
object-image points
Definition: dls.h:194
s
GLdouble s
Definition: glext.h:3602
src
GLuint src
Definition: glext.h:6303
mrpt::vision::pnp::dls::compute_eigenvec
void compute_eigenvec(const cv::Mat &Mtilde, cv::Mat &eigenval_real, cv::Mat &eigenval_imag, cv::Mat &eigenvec_real, cv::Mat &eigenvec_imag)
Eigen Value Decomposition.
mrpt::vision::pnp::dls::t_est_
std::vector< cv::Mat > t_est_
Definition: dls.h:196
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
mrpt::vision::pnp::dls::rotz
cv::Mat rotz(const double t)
Rotation matrix along z-axis by angle t.
mrpt::vision::pnp::dls::C_est_
std::vector< cv::Mat > C_est_
coefficient for coefficients matrix
Definition: dls.h:196
c
const GLubyte * c
Definition: glext.h:5590
g
GLubyte g
Definition: glext.h:5575
val
int val
Definition: mrpt_jpeglib.h:953
b
GLubyte GLubyte b
Definition: glext.h:5575
mrpt::vision::pnp::dls::cost_
std::vector< double > cost_
Definition: dls.h:195
mrpt::vision::pnp::dls::f3coeff
std::vector< double > f3coeff
Definition: dls.h:195
eps
const double eps
Definition: distributions_unittest.cpp:21
mrpt::vision::pnp::dls::cayley_LS_M
cv::Mat cayley_LS_M(const std::vector< double > &a, const std::vector< double > &b, const std::vector< double > &c, const std::vector< double > &u)
Fill the Maucaulay matrix with co-efficients.
mrpt::vision::pnp::dls::Hessian
cv::Mat Hessian(const double s[])
Compute the Hessian matrix for the polynomial co-efficient vector s.
mrpt::vision::pnp::dls::cost__
double cost__
optimal found solution
Definition: dls.h:198
mrpt::vision::pnp::dls::rotx
cv::Mat rotx(const double t)
Rotation matrix along x-axis by angle t.
mrpt::vision::pnp::dls::LeftMultVec
cv::Mat LeftMultVec(const cv::Mat &v)
Create a matrix from vector.
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::vision::pnp::dls::mean
cv::Mat mean(const cv::Mat &M)
Column-wise mean of matrix M.
mrpt::vision::pnp::dls::cayley2rotbar
cv::Mat cayley2rotbar(const cv::Mat &s)
Cayley parameters to Rotation Matrix.
mrpt::vision::pnp::dls::run_kernel
void run_kernel(const cv::Mat &pp)
Main function to run DLS-PnP.
y
GLenum GLint GLint y
Definition: glext.h:3516
x
GLenum GLint x
Definition: glext.h:3516
mrpt::vision::pnp::dls::init_points
void init_points(const cv::Mat &opoints, const cv::Mat &ipoints)
Initialization of object points and image points.
Definition: dls.h:53
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::vision::pnp::dls::dls
dls(const cv::Mat &opoints, const cv::Mat &ipoints)
Constructor for DLS class.
mrpt::vision::pnp::dls::positive_eigenvalues
bool positive_eigenvalues(const cv::Mat *eigenvalues)
check for positive eigenvalues
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:110
mrpt::vision::pnp::dls::skewsymm
cv::Mat skewsymm(const cv::Mat *X1)
Create a skwy-symmetric matrix from a vector.
mrpt::vision::pnp::dls::C_est__
cv::Mat C_est__
optimal candidates
Definition: dls.h:197


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019