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> init_points(const cv::Mat & opoints,const cv::Mat & ipoints)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> alloc_1d(int m)103 _Tp *alloc_1d(int m) { 104 return new _Tp[m]; 105 } 106 107 // Allocates memory. 108 template<typename _Tp> alloc_1d(int m,_Tp val)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> alloc_2d(int m,int _n)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> alloc_2d(int m,int _n,_Tp val)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 cdiv(double xr,double xi,double yr,double yi)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 hqr2()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; 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 double vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q; 533 double 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. orthes()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. release()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. compute()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: EigenvalueDecomposition()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). EigenvalueDecomposition(InputArray src)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). compute(InputArray src)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 ~EigenvalueDecomposition()765 ~EigenvalueDecomposition() {} 766 767 // Returns the eigenvalues of the Eigenvalue Decomposition. eigenvalues()768 Mat eigenvalues() { return _eigenvalues; } 769 // Returns the eigenvectors of the Eigenvalue Decomposition. eigenvectors()770 Mat eigenvectors() { return _eigenvectors; } 771 }; 772 773 #endif // DLS_H 774