modules/calib3d/src/dls.h

   1 #ifndef DLS_H_
   2 #define DLS_H_
   3
   4 #include "precomp.hpp"
   5
   6 #include <iostream>
   7
   8 using namespace std;
   9 using namespace cv;
  10
  11 class dls
  12 {
  13 public:
  14     dls(const cv::Mat& opoints, const cv::Mat& ipoints);
  15     ~dls();
  16
  17     bool compute_pose(cv::Mat& R, cv::Mat& t);
  18
  19 private:
  20
  21     // initialisation
  22     template <typename OpointType, typename IpointType>
  23     void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
  24     {
  25         for(int i = 0; i < N; i++)
  26         {
  27             p.at<double>(0,i) = opoints.at<OpointType>(i).x;
  28             p.at<double>(1,i) = opoints.at<OpointType>(i).y;
  29             p.at<double>(2,i) = opoints.at<OpointType>(i).z;
  30
  31             // compute mean of object points
  32             mn.at<double>(0) += p.at<double>(0,i);
  33             mn.at<double>(1) += p.at<double>(1,i);
  34             mn.at<double>(2) += p.at<double>(2,i);
  35
  36             // make z into unit vectors from normalized pixel coords
  37             double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
  38                         std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
  39                    sr = std::sqrt(sr);
  40
  41             z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
  42             z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
  43             z.at<double>(2,i) = (double)1 / sr;
  44         }
  45
  46         mn.at<double>(0) /= (double)N;
  47         mn.at<double>(1) /= (double)N;
  48         mn.at<double>(2) /= (double)N;
  49     }
  50
  51     // main algorithm
  52     cv::Mat LeftMultVec(const cv::Mat& v);
  53     void run_kernel(const cv::Mat& pp);
  54     void build_coeff_matrix(const cv::Mat& pp, cv::Mat& Mtilde, cv::Mat& D);
  55     void compute_eigenvec(const cv::Mat& Mtilde, cv::Mat& eigenval_real, cv::Mat& eigenval_imag,
  56                                                  cv::Mat& eigenvec_real, cv::Mat& eigenvec_imag);
  57     void fill_coeff(const cv::Mat * D);
  58
  59     // useful functions
  60     cv::Mat cayley_LS_M(const std::vector<double>& a, const std::vector<double>& b,
  61                         const std::vector<double>& c, const std::vector<double>& u);
  62     cv::Mat Hessian(const double s[]);
  63     cv::Mat cayley2rotbar(const cv::Mat& s);
  64     cv::Mat skewsymm(const cv::Mat * X1);
  65
  66     // extra functions
  67     cv::Mat rotx(const double t);
  68     cv::Mat roty(const double t);
  69     cv::Mat rotz(const double t);
  70     cv::Mat mean(const cv::Mat& M);
  71     bool is_empty(const cv::Mat * v);
  72     bool positive_eigenvalues(const cv::Mat * eigenvalues);
  73
  74     cv::Mat p, z, mn;        // object-image points
  75     int N;                // number of input points
  76     std::vector<double> f1coeff, f2coeff, f3coeff, cost_; // coefficient for coefficients matrix
  77     std::vector<cv::Mat> C_est_, t_est_;    // optimal candidates
  78     cv::Mat C_est__, t_est__;                // optimal found solution
  79     double cost__;                            // optimal found solution
  80 };
  81
  82 class EigenvalueDecomposition {
  83 private:
  84
  85     // Holds the data dimension.
  86     int n;
  87
  88     // Stores real/imag part of a complex division.
  89     double cdivr, cdivi;
  90
  91     // Pointer to internal memory.
  92     double *d, *e, *ort;
  93     double **V, **H;
  94
  95     // Holds the computed eigenvalues.
  96     Mat _eigenvalues;
  97
  98     // Holds the computed eigenvectors.
  99     Mat _eigenvectors;
 100
 101     // Allocates memory.
 102     template<typename _Tp>
 103     _Tp *alloc_1d(int m) {
 104         return new _Tp[m];
 105     }
 106
 107     // Allocates memory.
 108     template<typename _Tp>
 109     _Tp *alloc_1d(int m, _Tp val) {
 110         _Tp *arr = alloc_1d<_Tp> (m);
 111         for (int i = 0; i < m; i++)
 112             arr[i] = val;
 113         return arr;
 114     }
 115
 116     // Allocates memory.
 117     template<typename _Tp>
 118     _Tp **alloc_2d(int m, int _n) {
 119         _Tp **arr = new _Tp*[m];
 120         for (int i = 0; i < m; i++)
 121             arr[i] = new _Tp[_n];
 122         return arr;
 123     }
 124
 125     // Allocates memory.
 126     template<typename _Tp>
 127     _Tp **alloc_2d(int m, int _n, _Tp val) {
 128         _Tp **arr = alloc_2d<_Tp> (m, _n);
 129         for (int i = 0; i < m; i++) {
 130             for (int j = 0; j < _n; j++) {
 131                 arr[i][j] = val;
 132             }
 133         }
 134         return arr;
 135     }
 136
 137     void cdiv(double xr, double xi, double yr, double yi) {
 138         double r, dv;
 139         if (std::abs(yr) > std::abs(yi)) {
 140             r = yi / yr;
 141             dv = yr + r * yi;
 142             cdivr = (xr + r * xi) / dv;
 143             cdivi = (xi - r * xr) / dv;
 144         } else {
 145             r = yr / yi;
 146             dv = yi + r * yr;
 147             cdivr = (r * xr + xi) / dv;
 148             cdivi = (r * xi - xr) / dv;
 149         }
 150     }
 151
 152     // Nonsymmetric reduction from Hessenberg to real Schur form.
 153
 154     void hqr2() {
 155
 156         //  This is derived from the Algol procedure hqr2,
 157         //  by Martin and Wilkinson, Handbook for Auto. Comp.,
 158         //  Vol.ii-Linear Algebra, and the corresponding
 159         //  Fortran subroutine in EISPACK.
 160
 161         // Initialize
 162         int nn = this->n;
 163         int n1 = nn - 1;
 164         int low = 0;
 165         int high = nn - 1;
 166         double eps = std::pow(2.0, -52.0);
 167         double exshift = 0.0;
 168         double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;
 169
 170         // Store roots isolated by balanc and compute matrix norm
 171
 172         double norm = 0.0;
 173         for (int i = 0; i < nn; i++) {
 174             if (i < low || i > high) {
 175                 d[i] = H[i][i];
 176                 e[i] = 0.0;
 177             }
 178             for (int j = std::max(i - 1, 0); j < nn; j++) {
 179                 norm = norm + std::abs(H[i][j]);
 180             }
 181         }
 182
 183         // Outer loop over eigenvalue index
 184         int iter = 0;
 185         while (n1 >= low) {
 186
 187             // Look for single small sub-diagonal element
 188             int l = n1;
 189             while (l > low) {
 190                 s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
 191                 if (s == 0.0) {
 192                     s = norm;
 193                 }
 194                 if (std::abs(H[l][l - 1]) < eps * s) {
 195                     break;
 196                 }
 197                 l--;
 198             }
 199
 200             // Check for convergence
 201             // One root found
 202
 203             if (l == n1) {
 204                 H[n1][n1] = H[n1][n1] + exshift;
 205                 d[n1] = H[n1][n1];
 206                 e[n1] = 0.0;
 207                 n1--;
 208                 iter = 0;
 209
 210                 // Two roots found
 211
 212             } else if (l == n1 - 1) {
 213                 w = H[n1][n1 - 1] * H[n1 - 1][n1];
 214                 p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0;
 215                 q = p * p + w;
 216                 z = std::sqrt(std::abs(q));
 217                 H[n1][n1] = H[n1][n1] + exshift;
 218                 H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift;
 219                 x = H[n1][n1];
 220
 221                 // Real pair
 222
 223                 if (q >= 0) {
 224                     if (p >= 0) {
 225                         z = p + z;
 226                     } else {
 227                         z = p - z;
 228                     }
 229                     d[n1 - 1] = x + z;
 230                     d[n1] = d[n1 - 1];
 231                     if (z != 0.0) {
 232                         d[n1] = x - w / z;
 233                     }
 234                     e[n1 - 1] = 0.0;
 235                     e[n1] = 0.0;
 236                     x = H[n1][n1 - 1];
 237                     s = std::abs(x) + std::abs(z);
 238                     p = x / s;
 239                     q = z / s;
 240                     r = std::sqrt(p * p + q * q);
 241                     p = p / r;
 242                     q = q / r;
 243
 244                     // Row modification
 245
 246                     for (int j = n1 - 1; j < nn; j++) {
 247                         z = H[n1 - 1][j];
 248                         H[n1 - 1][j] = q * z + p * H[n1][j];
 249                         H[n1][j] = q * H[n1][j] - p * z;
 250                     }
 251
 252                     // Column modification
 253
 254                     for (int i = 0; i <= n1; i++) {
 255                         z = H[i][n1 - 1];
 256                         H[i][n1 - 1] = q * z + p * H[i][n1];
 257                         H[i][n1] = q * H[i][n1] - p * z;
 258                     }
 259
 260                     // Accumulate transformations
 261
 262                     for (int i = low; i <= high; i++) {
 263                         z = V[i][n1 - 1];
 264                         V[i][n1 - 1] = q * z + p * V[i][n1];
 265                         V[i][n1] = q * V[i][n1] - p * z;
 266                     }
 267
 268                     // Complex pair
 269
 270                 } else {
 271                     d[n1 - 1] = x + p;
 272                     d[n1] = x + p;
 273                     e[n1 - 1] = z;
 274                     e[n1] = -z;
 275                 }
 276                 n1 = n1 - 2;
 277                 iter = 0;
 278
 279                 // No convergence yet
 280
 281             } else {
 282
 283                 // Form shift
 284
 285                 x = H[n1][n1];
 286                 y = 0.0;
 287                 w = 0.0;
 288                 if (l < n1) {
 289                     y = H[n1 - 1][n1 - 1];
 290                     w = H[n1][n1 - 1] * H[n1 - 1][n1];
 291                 }
 292
 293                 // Wilkinson's original ad hoc shift
 294
 295                 if (iter == 10) {
 296                     exshift += x;
 297                     for (int i = low; i <= n1; i++) {
 298                         H[i][i] -= x;
 299                     }
 300                     s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]);
 301                     x = y = 0.75 * s;
 302                     w = -0.4375 * s * s;
 303                 }
 304
 305                 // MATLAB's new ad hoc shift
 306
 307                 if (iter == 30) {
 308                     s = (y - x) / 2.0;
 309                     s = s * s + w;
 310                     if (s > 0) {
 311                         s = std::sqrt(s);
 312                         if (y < x) {
 313                             s = -s;
 314                         }
 315                         s = x - w / ((y - x) / 2.0 + s);
 316                         for (int i = low; i <= n1; i++) {
 317                             H[i][i] -= s;
 318                         }
 319                         exshift += s;
 320                         x = y = w = 0.964;
 321                     }
 322                 }
 323
 324                 iter = iter + 1; // (Could check iteration count here.)
 325
 326                 // Look for two consecutive small sub-diagonal elements
 327                 int m = n1 - 2;
 328                 while (m >= l) {
 329                     z = H[m][m];
 330                     r = x - z;
 331                     s = y - z;
 332                     p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
 333                     q = H[m + 1][m + 1] - z - r - s;
 334                     r = H[m + 2][m + 1];
 335                     s = std::abs(p) + std::abs(q) + std::abs(r);
 336                     p = p / s;
 337                     q = q / s;
 338                     r = r / s;
 339                     if (m == l) {
 340                         break;
 341                     }
 342                     if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p)
 343                                                                                      * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(
 344                                                                                                                                            H[m + 1][m + 1])))) {
 345                         break;
 346                     }
 347                     m--;
 348                 }
 349
 350                 for (int i = m + 2; i <= n1; i++) {
 351                     H[i][i - 2] = 0.0;
 352                     if (i > m + 2) {
 353                         H[i][i - 3] = 0.0;
 354                     }
 355                 }
 356
 357                 // Double QR step involving rows l:n and columns m:n
 358
 359                 for (int k = m; k <= n1 - 1; k++) {
 360                     bool notlast = (k != n1 - 1);
 361                     if (k != m) {
 362                         p = H[k][k - 1];
 363                         q = H[k + 1][k - 1];
 364                         r = (notlast ? H[k + 2][k - 1] : 0.0);
 365                         x = std::abs(p) + std::abs(q) + std::abs(r);
 366                         if (x != 0.0) {
 367                             p = p / x;
 368                             q = q / x;
 369                             r = r / x;
 370                         }
 371                     }
 372                     if (x == 0.0) {
 373                         break;
 374                     }
 375                     s = std::sqrt(p * p + q * q + r * r);
 376                     if (p < 0) {
 377                         s = -s;
 378                     }
 379                     if (s != 0) {
 380                         if (k != m) {
 381                             H[k][k - 1] = -s * x;
 382                         } else if (l != m) {
 383                             H[k][k - 1] = -H[k][k - 1];
 384                         }
 385                         p = p + s;
 386                         x = p / s;
 387                         y = q / s;
 388                         z = r / s;
 389                         q = q / p;
 390                         r = r / p;
 391
 392                         // Row modification
 393
 394                         for (int j = k; j < nn; j++) {
 395                             p = H[k][j] + q * H[k + 1][j];
 396                             if (notlast) {
 397                                 p = p + r * H[k + 2][j];
 398                                 H[k + 2][j] = H[k + 2][j] - p * z;
 399                             }
 400                             H[k][j] = H[k][j] - p * x;
 401                             H[k + 1][j] = H[k + 1][j] - p * y;
 402                         }
 403
 404                         // Column modification
 405
 406                         for (int i = 0; i <= std::min(n1, k + 3); i++) {
 407                             p = x * H[i][k] + y * H[i][k + 1];
 408                             if (notlast) {
 409                                 p = p + z * H[i][k + 2];
 410                                 H[i][k + 2] = H[i][k + 2] - p * r;
 411                             }
 412                             H[i][k] = H[i][k] - p;
 413                             H[i][k + 1] = H[i][k + 1] - p * q;
 414                         }
 415
 416                         // Accumulate transformations
 417
 418                         for (int i = low; i <= high; i++) {
 419                             p = x * V[i][k] + y * V[i][k + 1];
 420                             if (notlast) {
 421                                 p = p + z * V[i][k + 2];
 422                                 V[i][k + 2] = V[i][k + 2] - p * r;
 423                             }
 424                             V[i][k] = V[i][k] - p;
 425                             V[i][k + 1] = V[i][k + 1] - p * q;
 426                         }
 427                     } // (s != 0)
 428                 } // k loop
 429             } // check convergence
 430         } // while (n1 >= low)
 431
 432         // Backsubstitute to find vectors of upper triangular form
 433
 434         if (norm == 0.0) {
 435             return;
 436         }
 437
 438         for (n1 = nn - 1; n1 >= 0; n1--) {
 439             p = d[n1];
 440             q = e[n1];
 441
 442             // Real vector
 443
 444             if (q == 0) {
 445                 int l = n1;
 446                 H[n1][n1] = 1.0;
 447                 for (int i = n1 - 1; i >= 0; i--) {
 448                     w = H[i][i] - p;
 449                     r = 0.0;
 450                     for (int j = l; j <= n1; j++) {
 451                         r = r + H[i][j] * H[j][n1];
 452                     }
 453                     if (e[i] < 0.0) {
 454                         z = w;
 455                         s = r;
 456                     } else {
 457                         l = i;
 458                         if (e[i] == 0.0) {
 459                             if (w != 0.0) {
 460                                 H[i][n1] = -r / w;
 461                             } else {
 462                                 H[i][n1] = -r / (eps * norm);
 463                             }
 464
 465                             // Solve real equations
 466
 467                         } else {
 468                             x = H[i][i + 1];
 469                             y = H[i + 1][i];
 470                             q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
 471                             t = (x * s - z * r) / q;
 472                             H[i][n1] = t;
 473                             if (std::abs(x) > std::abs(z)) {
 474                                 H[i + 1][n1] = (-r - w * t) / x;
 475                             } else {
 476                                 H[i + 1][n1] = (-s - y * t) / z;
 477                             }
 478                         }
 479
 480                         // Overflow control
 481
 482                         t = std::abs(H[i][n1]);
 483                         if ((eps * t) * t > 1) {
 484                             for (int j = i; j <= n1; j++) {
 485                                 H[j][n1] = H[j][n1] / t;
 486                             }
 487                         }
 488                     }
 489                 }
 490                 // Complex vector
 491             } else if (q < 0) {
 492                 int l = n1 - 1;
 493
 494                 // Last vector component imaginary so matrix is triangular
 495
 496                 if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) {
 497                     H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1];
 498                     H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1];
 499                 } else {
 500                     cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q);
 501                     H[n1 - 1][n1 - 1] = cdivr;
 502                     H[n1 - 1][n1] = cdivi;
 503                 }
 504                 H[n1][n1 - 1] = 0.0;
 505                 H[n1][n1] = 1.0;
 506                 for (int i = n1 - 2; i >= 0; i--) {
 507                     double ra, sa, vr, vi;
 508                     ra = 0.0;
 509                     sa = 0.0;
 510                     for (int j = l; j <= n1; j++) {
 511                         ra = ra + H[i][j] * H[j][n1 - 1];
 512                         sa = sa + H[i][j] * H[j][n1];
 513                     }
 514                     w = H[i][i] - p;
 515
 516                     if (e[i] < 0.0) {
 517                         z = w;
 518                         r = ra;
 519                         s = sa;
 520                     } else {
 521                         l = i;
 522                         if (e[i] == 0) {
 523                             cdiv(-ra, -sa, w, q);
 524                             H[i][n1 - 1] = cdivr;
 525                             H[i][n1] = cdivi;
 526                         } else {
 527
 528                             // Solve complex equations
 529
 530                             x = H[i][i + 1];
 531                             y = H[i + 1][i];
 532                             vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
 533                             vi = (d[i] - p) * 2.0 * q;
 534                             if (vr == 0.0 && vi == 0.0) {
 535                                 vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x)
 536                                                    + std::abs(y) + std::abs(z));
 537                             }
 538                             cdiv(x * r - z * ra + q * sa,
 539                                  x * s - z * sa - q * ra, vr, vi);
 540                             H[i][n1 - 1] = cdivr;
 541                             H[i][n1] = cdivi;
 542                             if (std::abs(x) > (std::abs(z) + std::abs(q))) {
 543                                 H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q
 544                                                    * H[i][n1]) / x;
 545                                 H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1
 546                                                                             - 1]) / x;
 547                             } else {
 548                                 cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z,
 549                                      q);
 550                                 H[i + 1][n1 - 1] = cdivr;
 551                                 H[i + 1][n1] = cdivi;
 552                             }
 553                         }
 554
 555                         // Overflow control
 556
 557                         t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1]));
 558                         if ((eps * t) * t > 1) {
 559                             for (int j = i; j <= n1; j++) {
 560                                 H[j][n1 - 1] = H[j][n1 - 1] / t;
 561                                 H[j][n1] = H[j][n1] / t;
 562                             }
 563                         }
 564                     }
 565                 }
 566             }
 567         }
 568
 569         // Vectors of isolated roots
 570
 571         for (int i = 0; i < nn; i++) {
 572             if (i < low || i > high) {
 573                 for (int j = i; j < nn; j++) {
 574                     V[i][j] = H[i][j];
 575                 }
 576             }
 577         }
 578
 579         // Back transformation to get eigenvectors of original matrix
 580
 581         for (int j = nn - 1; j >= low; j--) {
 582             for (int i = low; i <= high; i++) {
 583                 z = 0.0;
 584                 for (int k = low; k <= std::min(j, high); k++) {
 585                     z = z + V[i][k] * H[k][j];
 586                 }
 587                 V[i][j] = z;
 588             }
 589         }
 590     }
 591
 592     // Nonsymmetric reduction to Hessenberg form.
 593     void orthes() {
 594         //  This is derived from the Algol procedures orthes and ortran,
 595         //  by Martin and Wilkinson, Handbook for Auto. Comp.,
 596         //  Vol.ii-Linear Algebra, and the corresponding
 597         //  Fortran subroutines in EISPACK.
 598         int low = 0;
 599         int high = n - 1;
 600
 601         for (int m = low + 1; m <= high - 1; m++) {
 602
 603             // Scale column.
 604
 605             double scale = 0.0;
 606             for (int i = m; i <= high; i++) {
 607                 scale = scale + std::abs(H[i][m - 1]);
 608             }
 609             if (scale != 0.0) {
 610
 611                 // Compute Householder transformation.
 612
 613                 double h = 0.0;
 614                 for (int i = high; i >= m; i--) {
 615                     ort[i] = H[i][m - 1] / scale;
 616                     h += ort[i] * ort[i];
 617                 }
 618                 double g = std::sqrt(h);
 619                 if (ort[m] > 0) {
 620                     g = -g;
 621                 }
 622                 h = h - ort[m] * g;
 623                 ort[m] = ort[m] - g;
 624
 625                 // Apply Householder similarity transformation
 626                 // H = (I-u*u'/h)*H*(I-u*u')/h)
 627
 628                 for (int j = m; j < n; j++) {
 629                     double f = 0.0;
 630                     for (int i = high; i >= m; i--) {
 631                         f += ort[i] * H[i][j];
 632                     }
 633                     f = f / h;
 634                     for (int i = m; i <= high; i++) {
 635                         H[i][j] -= f * ort[i];
 636                     }
 637                 }
 638
 639                 for (int i = 0; i <= high; i++) {
 640                     double f = 0.0;
 641                     for (int j = high; j >= m; j--) {
 642                         f += ort[j] * H[i][j];
 643                     }
 644                     f = f / h;
 645                     for (int j = m; j <= high; j++) {
 646                         H[i][j] -= f * ort[j];
 647                     }
 648                 }
 649                 ort[m] = scale * ort[m];
 650                 H[m][m - 1] = scale * g;
 651             }
 652         }
 653
 654         // Accumulate transformations (Algol's ortran).
 655
 656         for (int i = 0; i < n; i++) {
 657             for (int j = 0; j < n; j++) {
 658                 V[i][j] = (i == j ? 1.0 : 0.0);
 659             }
 660         }
 661
 662         for (int m = high - 1; m >= low + 1; m--) {
 663             if (H[m][m - 1] != 0.0) {
 664                 for (int i = m + 1; i <= high; i++) {
 665                     ort[i] = H[i][m - 1];
 666                 }
 667                 for (int j = m; j <= high; j++) {
 668                     double g = 0.0;
 669                     for (int i = m; i <= high; i++) {
 670                         g += ort[i] * V[i][j];
 671                     }
 672                     // Double division avoids possible underflow
 673                     g = (g / ort[m]) / H[m][m - 1];
 674                     for (int i = m; i <= high; i++) {
 675                         V[i][j] += g * ort[i];
 676                     }
 677                 }
 678             }
 679         }
 680     }
 681
 682     // Releases all internal working memory.
 683     void release() {
 684         // releases the working data
 685         delete[] d;
 686         delete[] e;
 687         delete[] ort;
 688         for (int i = 0; i < n; i++) {
 689             delete[] H[i];
 690             delete[] V[i];
 691         }
 692         delete[] H;
 693         delete[] V;
 694     }
 695
 696     // Computes the Eigenvalue Decomposition for a matrix given in H.
 697     void compute() {
 698         // Allocate memory for the working data.
 699         V = alloc_2d<double> (n, n, 0.0);
 700         d = alloc_1d<double> (n);
 701         e = alloc_1d<double> (n);
 702         ort = alloc_1d<double> (n);
 703         // Reduce to Hessenberg form.
 704         orthes();
 705         // Reduce Hessenberg to real Schur form.
 706         hqr2();
 707         // Copy eigenvalues to OpenCV Matrix.
 708         _eigenvalues.create(1, n, CV_64FC1);
 709         for (int i = 0; i < n; i++) {
 710             _eigenvalues.at<double> (0, i) = d[i];
 711         }
 712         // Copy eigenvectors to OpenCV Matrix.
 713         _eigenvectors.create(n, n, CV_64FC1);
 714         for (int i = 0; i < n; i++)
 715             for (int j = 0; j < n; j++)
 716                 _eigenvectors.at<double> (i, j) = V[i][j];
 717         // Deallocate the memory by releasing all internal working data.
 718         release();
 719     }
 720
 721 public:
 722     EigenvalueDecomposition()
 723     : n(0) { }
 724
 725     // Initializes & computes the Eigenvalue Decomposition for a general matrix
 726     // given in src. This function is a port of the EigenvalueSolver in JAMA,
 727     // which has been released to public domain by The MathWorks and the
 728     // National Institute of Standards and Technology (NIST).
 729     EigenvalueDecomposition(InputArray src) {
 730         compute(src);
 731     }
 732
 733     // This function computes the Eigenvalue Decomposition for a general matrix
 734     // given in src. This function is a port of the EigenvalueSolver in JAMA,
 735     // which has been released to public domain by The MathWorks and the
 736     // National Institute of Standards and Technology (NIST).
 737     void compute(InputArray src)
 738     {
 739         /*if(isSymmetric(src)) {
 740             // Fall back to OpenCV for a symmetric matrix!
 741             cv::eigen(src, _eigenvalues, _eigenvectors);
 742         } else {*/
 743             Mat tmp;
 744             // Convert the given input matrix to double. Is there any way to
 745             // prevent allocating the temporary memory? Only used for copying
 746             // into working memory and deallocated after.
 747             src.getMat().convertTo(tmp, CV_64FC1);
 748             // Get dimension of the matrix.
 749             this->n = tmp.cols;
 750             // Allocate the matrix data to work on.
 751             this->H = alloc_2d<double> (n, n);
 752             // Now safely copy the data.
 753             for (int i = 0; i < tmp.rows; i++) {
 754                 for (int j = 0; j < tmp.cols; j++) {
 755                     this->H[i][j] = tmp.at<double>(i, j);
 756                 }
 757             }
 758             // Deallocates the temporary matrix before computing.
 759             tmp.release();
 760             // Performs the eigenvalue decomposition of H.
 761             compute();
 762        // }
 763     }
 764
 765     ~EigenvalueDecomposition() {}
 766
 767     // Returns the eigenvalues of the Eigenvalue Decomposition.
 768     Mat eigenvalues() {  return _eigenvalues; }
 769     // Returns the eigenvectors of the Eigenvalue Decomposition.
 770     Mat eigenvectors() { return _eigenvectors; }
 771 };
 772
 773 #endif // DLS_H