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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019