Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/LinearAlgebra.cpp @ 169:7105e51e4907 wasm
InvertMatrixUnsafe
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Fri, 02 Mar 2018 12:35:27 +0100 |
parents | 4f661e2f7b6c |
children | ff8556874557 |
comparison
equal
deleted
inserted
replaced
168:62964bfbbc00 | 169:7105e51e4907 |
---|---|
495 { | 495 { |
496 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | 496 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
497 } | 497 } |
498 } | 498 } |
499 | 499 |
500 | 500 |
501 void InvertMatrix(Matrix& target, | 501 bool InvertMatrixUnsafe(Matrix& target, |
502 const Matrix& source) | 502 const Matrix& source) |
503 { | 503 { |
504 if (source.size1() != source.size2()) | 504 if (source.size1() != source.size2()) |
505 { | 505 { |
506 LOG(ERROR) << "Inverse only exists for square matrices"; | 506 LOG(ERROR) << "Inverse only exists for square matrices"; |
507 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 507 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
514 | 514 |
515 if (source.size1() == 0) | 515 if (source.size1() == 0) |
516 { | 516 { |
517 // By convention, the inverse of the empty matrix, is itself the empty matrix | 517 // By convention, the inverse of the empty matrix, is itself the empty matrix |
518 target.resize(0, 0); | 518 target.resize(0, 0); |
519 return; | 519 return true; |
520 } | 520 } |
521 | 521 |
522 double determinant = ComputeDeterminant(source); | 522 double determinant = ComputeDeterminant(source); |
523 | 523 |
524 if (IsCloseToZero(determinant)) | 524 if (IsCloseToZero(determinant)) |
525 { | 525 { |
526 LOG(ERROR) << "Cannot invert singular matrix"; | 526 return false; |
527 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
528 } | 527 } |
529 | 528 |
530 double denominator = 1.0 / determinant; | 529 double denominator = 1.0 / determinant; |
531 | 530 |
532 target.resize(source.size1(), source.size2()); | 531 target.resize(source.size1(), source.size2()); |
533 | 532 |
534 if (source.size1() == 1) | 533 if (source.size1() == 1) |
535 { | 534 { |
536 target(0, 0) = denominator; | 535 target(0, 0) = denominator; |
536 | |
537 return true; | |
537 } | 538 } |
538 else if (source.size1() == 2) | 539 else if (source.size1() == 2) |
539 { | 540 { |
540 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices | 541 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices |
541 target(0, 0) = source(1, 1) * denominator; | 542 target(0, 0) = source(1, 1) * denominator; |
542 target(0, 1) = -source(0, 1) * denominator; | 543 target(0, 1) = -source(0, 1) * denominator; |
543 target(1, 0) = -source(1, 0) * denominator; | 544 target(1, 0) = -source(1, 0) * denominator; |
544 target(1, 1) = source(0, 0) * denominator; | 545 target(1, 1) = source(0, 0) * denominator; |
546 | |
547 return true; | |
545 } | 548 } |
546 else if (source.size1() == 3) | 549 else if (source.size1() == 3) |
547 { | 550 { |
548 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices | 551 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices |
549 const double a = source(0, 0); | 552 const double a = source(0, 0); |
563 target(1, 1) = (a * i - c * g) * denominator; | 566 target(1, 1) = (a * i - c * g) * denominator; |
564 target(1, 2) = -(a * f - c * d) * denominator; | 567 target(1, 2) = -(a * f - c * d) * denominator; |
565 target(2, 0) = (d * h - e * g) * denominator; | 568 target(2, 0) = (d * h - e * g) * denominator; |
566 target(2, 1) = -(a * h - b * g) * denominator; | 569 target(2, 1) = -(a * h - b * g) * denominator; |
567 target(2, 2) = (a * e - b * d) * denominator; | 570 target(2, 2) = (a * e - b * d) * denominator; |
571 | |
572 return true; | |
568 } | 573 } |
569 else | 574 else |
570 { | 575 { |
571 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | 576 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); |
572 } | 577 } |
579 | 584 |
580 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1()); | 585 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1()); |
581 | 586 |
582 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) | 587 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) |
583 { | 588 { |
584 LOG(ERROR) << "Cannot invert singular matrix"; | 589 return false; |
585 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 590 } |
586 } | 591 else |
587 | 592 { |
588 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); | 593 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); |
589 lu_substitute(a, permutation, target); | 594 lu_substitute(a, permutation, target); |
595 return true; | |
596 } | |
597 } | |
598 } | |
599 | |
600 | |
601 | |
602 void InvertMatrix(Matrix& target, | |
603 const Matrix& source) | |
604 { | |
605 if (!InvertMatrixUnsafe(target, source)) | |
606 { | |
607 LOG(ERROR) << "Cannot invert singular matrix"; | |
608 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
590 } | 609 } |
591 } | 610 } |
592 | 611 |
593 | 612 |
594 void CreateSkewSymmetric(Matrix& s, | 613 void CreateSkewSymmetric(Matrix& s, |