Mercurial > hg > orthanc-stone

comparison Framework/Toolbox/LinearAlgebra.cpp @ 164:432b1f812d14 wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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 }