Mercurial > hg > orthanc-stone
changeset 163:8c5b24892ed2 wasm
LinearAlgebra::InvertMatrix
| author | Sebastien Jodogne <s.jodogne@gmail.com> |
|---|---|
| date | Wed, 14 Feb 2018 14:26:26 +0100 |
| parents | 77715c340767 |
| children | 432b1f812d14 |
| files | Framework/Toolbox/LinearAlgebra.cpp Framework/Toolbox/LinearAlgebra.h UnitTestsSources/UnitTestsMain.cpp |
| diffstat | 3 files changed, 181 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- a/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 12:24:56 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 14:26:26 2018 +0100 @@ -449,5 +449,68 @@ throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); } } + + + void InvertMatrix(Matrix& target, + const Matrix& source) + { + if (source.size1() != source.size2()) + { + LOG(ERROR) << "Inverse only exists for square matrices"; + 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) + { + // 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; + } + else + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + } + } } }
--- a/Framework/Toolbox/LinearAlgebra.h Wed Feb 14 12:24:56 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.h Wed Feb 14 14:26:26 2018 +0100 @@ -124,5 +124,8 @@ void RQDecomposition3x3(Matrix& r, Matrix& q, const Matrix& a); + + void InvertMatrix(Matrix& target, + const Matrix& source); }; }
--- a/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 12:24:56 2018 +0100 +++ b/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 14:26:26 2018 +0100 @@ -437,6 +437,121 @@ } +TEST(Matrix, Inverse1) +{ + OrthancStone::Matrix a, b; + + ASSERT_THROW(OrthancStone::LinearAlgebra::InvertMatrix(b, a), Orthanc::OrthancException); + + a.resize(2, 3); + ASSERT_THROW(OrthancStone::LinearAlgebra::InvertMatrix(b, a), Orthanc::OrthancException); + + a.resize(1, 1); + a(0, 0) = 45.0; + + ASSERT_DOUBLE_EQ(45, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(1u, b.size1()); + ASSERT_EQ(1u, b.size2()); + ASSERT_DOUBLE_EQ(1.0 / 45.0, b(0, 0)); + + a(0, 0) = 0; + ASSERT_DOUBLE_EQ(0, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + ASSERT_THROW(OrthancStone::LinearAlgebra::InvertMatrix(b, a), Orthanc::OrthancException); +} + + +TEST(Matrix, Inverse2) +{ + OrthancStone::Matrix a, b; + a.resize(2, 2); + a(0, 0) = 4; + a(0, 1) = 3; + a(1, 0) = 3; + a(1, 1) = 2; + + ASSERT_DOUBLE_EQ(-1, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(2u, b.size1()); + ASSERT_EQ(2u, b.size2()); + + ASSERT_DOUBLE_EQ(-2, b(0, 0)); + ASSERT_DOUBLE_EQ(3, b(0, 1)); + ASSERT_DOUBLE_EQ(3, b(1, 0)); + ASSERT_DOUBLE_EQ(-4, b(1, 1)); + + a(0, 0) = 1; + a(0, 1) = 2; + a(1, 0) = 3; + a(1, 1) = 4; + + ASSERT_DOUBLE_EQ(-2, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + + ASSERT_DOUBLE_EQ(-2, b(0, 0)); + ASSERT_DOUBLE_EQ(1, b(0, 1)); + ASSERT_DOUBLE_EQ(1.5, b(1, 0)); + ASSERT_DOUBLE_EQ(-0.5, b(1, 1)); +} + + +TEST(Matrix, Inverse3) +{ + OrthancStone::Matrix a, b; + a.resize(3, 3); + a(0, 0) = 7; + a(0, 1) = 2; + a(0, 2) = 1; + a(1, 0) = 0; + a(1, 1) = 3; + a(1, 2) = -1; + a(2, 0) = -3; + a(2, 1) = 4; + a(2, 2) = -2; + + ASSERT_DOUBLE_EQ(1, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(3u, b.size1()); + ASSERT_EQ(3u, b.size2()); + + ASSERT_DOUBLE_EQ(-2, b(0, 0)); + ASSERT_DOUBLE_EQ(8, b(0, 1)); + ASSERT_DOUBLE_EQ(-5, b(0, 2)); + ASSERT_DOUBLE_EQ(3, b(1, 0)); + ASSERT_DOUBLE_EQ(-11, b(1, 1)); + ASSERT_DOUBLE_EQ(7, b(1, 2)); + ASSERT_DOUBLE_EQ(9, b(2, 0)); + ASSERT_DOUBLE_EQ(-34, b(2, 1)); + ASSERT_DOUBLE_EQ(21, b(2, 2)); + + + a(0, 0) = 1; + a(0, 1) = 2; + a(0, 2) = 2; + a(1, 0) = 1; + a(1, 1) = 0; + a(1, 2) = 1; + a(2, 0) = 1; + a(2, 1) = 2; + a(2, 2) = 1; + + ASSERT_DOUBLE_EQ(2, OrthancStone::LinearAlgebra::ComputeDeterminant(a)); + OrthancStone::LinearAlgebra::InvertMatrix(b, a); + ASSERT_EQ(3u, b.size1()); + ASSERT_EQ(3u, b.size2()); + + ASSERT_DOUBLE_EQ(-1, b(0, 0)); + ASSERT_DOUBLE_EQ(1, b(0, 1)); + ASSERT_DOUBLE_EQ(1, b(0, 2)); + ASSERT_DOUBLE_EQ(0, b(1, 0)); + ASSERT_DOUBLE_EQ(-0.5, b(1, 1)); + ASSERT_DOUBLE_EQ(0.5, b(1, 2)); + ASSERT_DOUBLE_EQ(1, b(2, 0)); + ASSERT_DOUBLE_EQ(0, b(2, 1)); + ASSERT_DOUBLE_EQ(-1, b(2, 2)); +} + + int main(int argc, char **argv) { Orthanc::Logging::Initialize();