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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9690a7a25 Mon Feb 24 17:42:02 2020 +0100 at lun feb 24 18:00:10 CET 2020