# HG changeset patch # User Sebastien Jodogne # Date 1518623383 -3600 # Node ID 432b1f812d14f0c480cf88ed1ab8c15281b287dc # Parent 8c5b24892ed201d104b784f0206cc43bef0cffb9 inversion of general matrices diff -r 8c5b24892ed2 -r 432b1f812d14 Framework/Toolbox/LinearAlgebra.cpp --- a/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 14:26:26 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 16:49:43 2018 +0100 @@ -27,6 +27,7 @@ #include #include +#include namespace OrthancStone { @@ -460,56 +461,86 @@ throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); } - double determinant = ComputeDeterminant(source); - - if (IsCloseToZero(determinant)) - { - LOG(ERROR) << "Cannot invert singular matrix"; - throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); - } - - double denominator = 1.0 / determinant; - - target.resize(source.size1(), source.size2()); - - if (source.size1() == 1) - { - target(0, 0) = denominator; - } - else if (source.size1() == 2) + if (source.size1() < 4) { - // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices - target(0, 0) = source(1, 1) * denominator; - target(0, 1) = -source(0, 1) * denominator; - target(1, 0) = -source(1, 0) * denominator; - target(1, 1) = source(0, 0) * denominator; - } - else if (source.size1() == 3) - { - // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - const double a = source(0, 0); - const double b = source(0, 1); - const double c = source(0, 2); - const double d = source(1, 0); - const double e = source(1, 1); - const double f = source(1, 2); - const double g = source(2, 0); - const double h = source(2, 1); - const double i = source(2, 2); + // For matrices with size below 4, use direct computations + // instead of LU decomposition + + if (source.size1() == 0) + { + // By convention, the inverse of the empty matrix, is itself the empty matrix + target.resize(0, 0); + return; + } + + double determinant = ComputeDeterminant(source); + + if (IsCloseToZero(determinant)) + { + LOG(ERROR) << "Cannot invert singular matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + double denominator = 1.0 / determinant; + + target.resize(source.size1(), source.size2()); + + if (source.size1() == 1) + { + target(0, 0) = denominator; + } + else if (source.size1() == 2) + { + // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices + target(0, 0) = source(1, 1) * denominator; + target(0, 1) = -source(0, 1) * denominator; + target(1, 0) = -source(1, 0) * denominator; + target(1, 1) = source(0, 0) * denominator; + } + else if (source.size1() == 3) + { + // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices + const double a = source(0, 0); + const double b = source(0, 1); + const double c = source(0, 2); + const double d = source(1, 0); + const double e = source(1, 1); + const double f = source(1, 2); + const double g = source(2, 0); + const double h = source(2, 1); + const double i = source(2, 2); - target(0, 0) = (e * i - f * h) * denominator; - target(0, 1) = -(b * i - c * h) * denominator; - target(0, 2) = (b * f - c * e) * denominator; - target(1, 0) = -(d * i - f * g) * denominator; - target(1, 1) = (a * i - c * g) * denominator; - target(1, 2) = -(a * f - c * d) * denominator; - target(2, 0) = (d * h - e * g) * denominator; - target(2, 1) = -(a * h - b * g) * denominator; - target(2, 2) = (a * e - b * d) * denominator; + target(0, 0) = (e * i - f * h) * denominator; + target(0, 1) = -(b * i - c * h) * denominator; + target(0, 2) = (b * f - c * e) * denominator; + target(1, 0) = -(d * i - f * g) * denominator; + target(1, 1) = (a * i - c * g) * denominator; + target(1, 2) = -(a * f - c * d) * denominator; + target(2, 0) = (d * h - e * g) * denominator; + target(2, 1) = -(a * h - b * g) * denominator; + target(2, 2) = (a * e - b * d) * denominator; + } + else + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); + } } else { - throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + // General case, using LU decomposition + + Matrix a = source; // Copy the source matrix, as "lu_factorize()" modifies it + + boost::numeric::ublas::permutation_matrix permutation(source.size1()); + + if (boost::numeric::ublas::lu_factorize(a, permutation) != 0) + { + LOG(ERROR) << "Cannot invert singular matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + target = boost::numeric::ublas::identity_matrix(source.size1()); + lu_substitute(a, permutation, target); } } } diff -r 8c5b24892ed2 -r 432b1f812d14 UnitTestsSources/UnitTestsMain.cpp --- a/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 14:26:26 2018 +0100 +++ b/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 16:49:43 2018 +0100 @@ -441,7 +441,10 @@ { OrthancStone::Matrix a, b; - ASSERT_THROW(OrthancStone::LinearAlgebra::InvertMatrix(b, a), Orthanc::OrthancException); + a.resize(0, 0); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(0u, b.size1()); + ASSERT_EQ(0u, b.size2()); a.resize(2, 3); ASSERT_THROW(OrthancStone::LinearAlgebra::InvertMatrix(b, a), Orthanc::OrthancException); @@ -552,6 +555,52 @@ } +TEST(Matrix, Inverse4) +{ + OrthancStone::Matrix a, b; + a.resize(4, 4); + a(0, 0) = 2; + a(0, 1) = 1; + a(0, 2) = 2; + a(0, 3) = -3; + a(1, 0) = -2; + a(1, 1) = 2; + a(1, 2) = -1; + a(1, 3) = -1; + a(2, 0) = 2; + a(2, 1) = 2; + a(2, 2) = -3; + a(2, 3) = -1; + a(3, 0) = 3; + a(3, 1) = -2; + a(3, 2) = -3; + a(3, 3) = -1; + + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(4u, b.size1()); + ASSERT_EQ(4u, b.size2()); + + b *= 134.0; // This is the determinant + + ASSERT_DOUBLE_EQ(8, b(0, 0)); + ASSERT_DOUBLE_EQ(-44, b(0, 1)); + ASSERT_DOUBLE_EQ(30, b(0, 2)); + ASSERT_DOUBLE_EQ(-10, b(0, 3)); + ASSERT_DOUBLE_EQ(2, b(1, 0)); + ASSERT_DOUBLE_EQ(-11, b(1, 1)); + ASSERT_DOUBLE_EQ(41, b(1, 2)); + ASSERT_DOUBLE_EQ(-36, b(1, 3)); + ASSERT_DOUBLE_EQ(16, b(2, 0)); + ASSERT_DOUBLE_EQ(-21, b(2, 1)); + ASSERT_DOUBLE_EQ(-7, b(2, 2)); + ASSERT_DOUBLE_EQ(-20, b(2, 3)); + ASSERT_DOUBLE_EQ(-28, b(3, 0)); + ASSERT_DOUBLE_EQ(-47, b(3, 1)); + ASSERT_DOUBLE_EQ(29, b(3, 2)); + ASSERT_DOUBLE_EQ(-32, b(3, 3)); +} + + int main(int argc, char **argv) { Orthanc::Logging::Initialize();