Mercurial > hg > orthanc-stone
comparison Framework/Toolbox/LinearAlgebra.cpp @ 164:432b1f812d14 wasm
inversion of general matrices
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 16:49:43 +0100 |
parents | 8c5b24892ed2 |
children | 8d50e6be565d |
comparison
equal
deleted
inserted
replaced
163:8c5b24892ed2 | 164:432b1f812d14 |
---|---|
25 #include <Core/OrthancException.h> | 25 #include <Core/OrthancException.h> |
26 #include <Core/Toolbox.h> | 26 #include <Core/Toolbox.h> |
27 | 27 |
28 #include <stdio.h> | 28 #include <stdio.h> |
29 #include <boost/lexical_cast.hpp> | 29 #include <boost/lexical_cast.hpp> |
30 #include <boost/numeric/ublas/lu.hpp> | |
30 | 31 |
31 namespace OrthancStone | 32 namespace OrthancStone |
32 { | 33 { |
33 namespace LinearAlgebra | 34 namespace LinearAlgebra |
34 { | 35 { |
458 { | 459 { |
459 LOG(ERROR) << "Inverse only exists for square matrices"; | 460 LOG(ERROR) << "Inverse only exists for square matrices"; |
460 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 461 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
461 } | 462 } |
462 | 463 |
463 double determinant = ComputeDeterminant(source); | 464 if (source.size1() < 4) |
464 | 465 { |
465 if (IsCloseToZero(determinant)) | 466 // For matrices with size below 4, use direct computations |
466 { | 467 // instead of LU decomposition |
467 LOG(ERROR) << "Cannot invert singular matrix"; | 468 |
468 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | 469 if (source.size1() == 0) |
469 } | 470 { |
470 | 471 // By convention, the inverse of the empty matrix, is itself the empty matrix |
471 double denominator = 1.0 / determinant; | 472 target.resize(0, 0); |
472 | 473 return; |
473 target.resize(source.size1(), source.size2()); | 474 } |
474 | 475 |
475 if (source.size1() == 1) | 476 double determinant = ComputeDeterminant(source); |
476 { | 477 |
477 target(0, 0) = denominator; | 478 if (IsCloseToZero(determinant)) |
478 } | 479 { |
479 else if (source.size1() == 2) | 480 LOG(ERROR) << "Cannot invert singular matrix"; |
480 { | 481 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); |
481 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices | 482 } |
482 target(0, 0) = source(1, 1) * denominator; | 483 |
483 target(0, 1) = -source(0, 1) * denominator; | 484 double denominator = 1.0 / determinant; |
484 target(1, 0) = -source(1, 0) * denominator; | 485 |
485 target(1, 1) = source(0, 0) * denominator; | 486 target.resize(source.size1(), source.size2()); |
486 } | 487 |
487 else if (source.size1() == 3) | 488 if (source.size1() == 1) |
488 { | 489 { |
489 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices | 490 target(0, 0) = denominator; |
490 const double a = source(0, 0); | 491 } |
491 const double b = source(0, 1); | 492 else if (source.size1() == 2) |
492 const double c = source(0, 2); | 493 { |
493 const double d = source(1, 0); | 494 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices |
494 const double e = source(1, 1); | 495 target(0, 0) = source(1, 1) * denominator; |
495 const double f = source(1, 2); | 496 target(0, 1) = -source(0, 1) * denominator; |
496 const double g = source(2, 0); | 497 target(1, 0) = -source(1, 0) * denominator; |
497 const double h = source(2, 1); | 498 target(1, 1) = source(0, 0) * denominator; |
498 const double i = source(2, 2); | 499 } |
500 else if (source.size1() == 3) | |
501 { | |
502 // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices | |
503 const double a = source(0, 0); | |
504 const double b = source(0, 1); | |
505 const double c = source(0, 2); | |
506 const double d = source(1, 0); | |
507 const double e = source(1, 1); | |
508 const double f = source(1, 2); | |
509 const double g = source(2, 0); | |
510 const double h = source(2, 1); | |
511 const double i = source(2, 2); | |
499 | 512 |
500 target(0, 0) = (e * i - f * h) * denominator; | 513 target(0, 0) = (e * i - f * h) * denominator; |
501 target(0, 1) = -(b * i - c * h) * denominator; | 514 target(0, 1) = -(b * i - c * h) * denominator; |
502 target(0, 2) = (b * f - c * e) * denominator; | 515 target(0, 2) = (b * f - c * e) * denominator; |
503 target(1, 0) = -(d * i - f * g) * denominator; | 516 target(1, 0) = -(d * i - f * g) * denominator; |
504 target(1, 1) = (a * i - c * g) * denominator; | 517 target(1, 1) = (a * i - c * g) * denominator; |
505 target(1, 2) = -(a * f - c * d) * denominator; | 518 target(1, 2) = -(a * f - c * d) * denominator; |
506 target(2, 0) = (d * h - e * g) * denominator; | 519 target(2, 0) = (d * h - e * g) * denominator; |
507 target(2, 1) = -(a * h - b * g) * denominator; | 520 target(2, 1) = -(a * h - b * g) * denominator; |
508 target(2, 2) = (a * e - b * d) * denominator; | 521 target(2, 2) = (a * e - b * d) * denominator; |
522 } | |
523 else | |
524 { | |
525 throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); | |
526 } | |
509 } | 527 } |
510 else | 528 else |
511 { | 529 { |
512 throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); | 530 // General case, using LU decomposition |
531 | |
532 Matrix a = source; // Copy the source matrix, as "lu_factorize()" modifies it | |
533 | |
534 boost::numeric::ublas::permutation_matrix<size_t> permutation(source.size1()); | |
535 | |
536 if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) | |
537 { | |
538 LOG(ERROR) << "Cannot invert singular matrix"; | |
539 throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); | |
540 } | |
541 | |
542 target = boost::numeric::ublas::identity_matrix<double>(source.size1()); | |
543 lu_substitute(a, permutation, target); | |
513 } | 544 } |
514 } | 545 } |
515 } | 546 } |
516 } | 547 } |