Mercurial > hg > orthanc-stone

changeset 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
files Framework/Toolbox/LinearAlgebra.cpp UnitTestsSources/UnitTestsMain.cpp
diffstat 2 files changed, 126 insertions(+), 46 deletions(-) [+]
line wrap: on
line diff
--- 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 <stdio.h>
 #include <boost/lexical_cast.hpp>
+#include <boost/numeric/ublas/lu.hpp>
 
 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<size_t>  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<double>(source.size1());
+        lu_substitute(a, permutation, target);
       }
     }
   }
--- 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();