Mercurial > hg > orthanc-stone

changeset 165:8d50e6be565d wasm

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
LinearAlgebra toolbox
author Sebastien Jodogne <s.jodogne@gmail.com>
date Thu, 15 Feb 2018 13:51:29 +0100
parents 432b1f812d14
children 4f661e2f7b6c
files Framework/Toolbox/GeometryToolbox.cpp Framework/Toolbox/GeometryToolbox.h Framework/Toolbox/LinearAlgebra.cpp Framework/Toolbox/LinearAlgebra.h UnitTestsSources/UnitTestsMain.cpp
diffstat 5 files changed, 347 insertions(+), 4 deletions(-) [+]
line wrap: on
line diff
--- a/Framework/Toolbox/GeometryToolbox.cpp	Wed Feb 14 16:49:43 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.cpp	Thu Feb 15 13:51:29 2018 +0100
@@ -328,6 +328,30 @@
     }    
 
 
+    Matrix CreateTranslationMatrix(double dx,
+                                   double dy,
+                                   double dz)
+    {
+      Matrix m = LinearAlgebra::IdentityMatrix(4);
+      m(0,3) = dx;
+      m(1,3) = dy;
+      m(2,3) = dz;
+      return m;    
+    }
+
+
+    Matrix CreateScalingMatrix(double sx,
+                               double sy,
+                               double sz)
+    {
+      Matrix m = LinearAlgebra::IdentityMatrix(4);
+      m(0,0) = sx;
+      m(1,1) = sy;
+      m(2,2) = sz;
+      return m;    
+    }
+
+
     bool IntersectPlaneAndSegment(Vector& p,
                                   const Vector& normal,
                                   double d,
--- a/Framework/Toolbox/GeometryToolbox.h	Wed Feb 14 16:49:43 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.h	Thu Feb 15 13:51:29 2018 +0100
@@ -75,6 +75,14 @@
 
     Matrix CreateRotationMatrixAlongZ(double a);
 
+    Matrix CreateTranslationMatrix(double dx,
+                                   double dy,
+                                   double dz);
+
+    Matrix CreateScalingMatrix(double sx,
+                               double sy,
+                               double sz);
+    
     bool IntersectPlaneAndSegment(Vector& p,
                                   const Vector& normal,
                                   double d,
--- a/Framework/Toolbox/LinearAlgebra.cpp	Wed Feb 14 16:49:43 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.cpp	Thu Feb 15 13:51:29 2018 +0100
@@ -105,6 +105,14 @@
 
 
     void AssignVector(Vector& v,
+                      double v1)
+    {
+      v.resize(1);
+      v[0] = v1;
+    }
+
+
+    void AssignVector(Vector& v,
                       double v1,
                       double v2,
                       double v3)
@@ -130,6 +138,44 @@
     }
 
 
+    Vector CreateVector(double v1)
+    {
+      Vector v;
+      AssignVector(v, v1);
+      return v;
+    }
+
+    
+    Vector CreateVector(double v1,
+                        double v2)
+    {
+      Vector v;
+      AssignVector(v, v1, v2);
+      return v;
+    }
+
+    
+    Vector CreateVector(double v1,
+                        double v2,
+                        double v3)
+    {
+      Vector v;
+      AssignVector(v, v1, v2, v3);
+      return v;
+    }
+
+    
+    Vector CreateVector(double v1,
+                        double v2,
+                        double v3,
+                        double v4)
+    {
+      Vector v;
+      AssignVector(v, v1, v2, v3, v4);
+      return v;
+    }
+
+
     bool IsNear(double x,
                 double y)
     {
@@ -543,5 +589,169 @@
         lu_substitute(a, permutation, target);
       }
     }
+
+    
+    void CreateSkewSymmetric(Matrix& s,
+                             const Vector& v)
+    {
+      assert(v.size() == 3);
+
+      s.resize(3, 3);
+      s(0,0) = 0;
+      s(0,1) = -v[2];
+      s(0,2) = v[1];
+      s(1,0) = v[2];
+      s(1,1) = 0;
+      s(1,2) = -v[0];
+      s(2,0) = -v[1];
+      s(2,1) = v[0];
+      s(2,2) = 0;
+    }
+
+  
+    void AlignVectorsWithRotation(Matrix& r,
+                                  const Vector& a,
+                                  const Vector& b)
+    {
+      // This is Rodrigues' rotation formula:
+      // https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula#Matrix_notation
+
+      // Check also result A4.6 from "Multiple View Geometry in Computer
+      // Vision - 2nd edition" (p. 584)
+  
+      if (a.size() != 3 ||
+          b.size() != 3)
+      {
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);
+      }
+
+      double aNorm = boost::numeric::ublas::norm_2(a);
+      double bNorm = boost::numeric::ublas::norm_2(b);
+
+      if (LinearAlgebra::IsCloseToZero(aNorm) ||
+          LinearAlgebra::IsCloseToZero(bNorm))
+      {
+        LOG(ERROR) << "Vector with zero norm";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange);      
+      }
+
+      Vector aUnit, bUnit;
+      aUnit = a / aNorm;
+      bUnit = b / bNorm;
+
+      Vector v;
+      LinearAlgebra::CrossProduct(v, aUnit, bUnit);
+
+      double cosine = boost::numeric::ublas::inner_prod(aUnit, bUnit);
+
+      if (LinearAlgebra::IsCloseToZero(1 + cosine))
+      {
+        // "a == -b": TODO
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented);
+      }
+
+      Matrix k;
+      CreateSkewSymmetric(k, v);
+
+#if 0
+      double sine = boost::numeric::ublas::norm_2(v);
+
+      r = (boost::numeric::ublas::identity_matrix<double>(3) +
+           sine * k + 
+           (1 - cosine) * boost::numeric::ublas::prod(k, k));
+#else
+      r = (boost::numeric::ublas::identity_matrix<double>(3) +
+           k + 
+           boost::numeric::ublas::prod(k, k) / (1 + cosine));
+#endif
+    }
+
+
+    Matrix InvertScaleTranslationMatrix(const Matrix& t)
+    {
+      if (t.size1() != 4 ||
+          t.size2() != 4 ||
+          !LinearAlgebra::IsCloseToZero(t(0,1)) ||
+          !LinearAlgebra::IsCloseToZero(t(0,2)) ||
+          !LinearAlgebra::IsCloseToZero(t(1,0)) ||
+          !LinearAlgebra::IsCloseToZero(t(1,2)) ||
+          !LinearAlgebra::IsCloseToZero(t(2,0)) ||
+          !LinearAlgebra::IsCloseToZero(t(2,1)) ||
+          !LinearAlgebra::IsCloseToZero(t(3,0)) ||
+          !LinearAlgebra::IsCloseToZero(t(3,1)) ||
+          !LinearAlgebra::IsCloseToZero(t(3,2)))
+      {
+        LOG(ERROR) << "This matrix is more than a zoom/translate transform";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
+      }
+
+      const double sx = t(0,0);
+      const double sy = t(1,1);
+      const double sz = t(2,2);
+      const double w = t(3,3);
+
+      if (LinearAlgebra::IsCloseToZero(sx) ||
+          LinearAlgebra::IsCloseToZero(sy) ||
+          LinearAlgebra::IsCloseToZero(sz) ||
+          LinearAlgebra::IsCloseToZero(w))
+      {
+        LOG(ERROR) << "Singular transform";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
+      }
+    
+      const double tx = t(0,3);
+      const double ty = t(1,3);
+      const double tz = t(2,3);
+    
+      Matrix m = IdentityMatrix(4);
+
+      m(0,0) = 1.0 / sx;
+      m(1,1) = 1.0 / sy;
+      m(2,2) = 1.0 / sz;
+      m(3,3) = 1.0 / w;
+
+      m(0,3) = -tx / (sx * w);
+      m(1,3) = -ty / (sy * w);
+      m(2,3) = -tz / (sz * w);
+
+      return m;
+    }
+
+
+    bool IsShearMatrix(const Matrix& shear)
+    {
+      return (shear.size1() == 4 &&
+              shear.size2() == 4 &&
+              LinearAlgebra::IsNear(1.0, shear(0,0)) &&
+              LinearAlgebra::IsNear(0.0, shear(0,1)) &&
+              LinearAlgebra::IsNear(0.0, shear(0,3)) &&
+              LinearAlgebra::IsNear(0.0, shear(1,0)) &&
+              LinearAlgebra::IsNear(1.0, shear(1,1)) &&
+              LinearAlgebra::IsNear(0.0, shear(1,3)) &&
+              LinearAlgebra::IsNear(0.0, shear(2,0)) &&
+              LinearAlgebra::IsNear(0.0, shear(2,1)) &&
+              LinearAlgebra::IsNear(1.0, shear(2,2)) &&
+              LinearAlgebra::IsNear(0.0, shear(2,3)) &&
+              LinearAlgebra::IsNear(0.0, shear(3,0)) &&
+              LinearAlgebra::IsNear(0.0, shear(3,1)) &&
+              LinearAlgebra::IsNear(1.0, shear(3,3)));
+    }
+  
+
+    Matrix InvertShearMatrix(const Matrix& shear)
+    {
+      if (!IsShearMatrix(shear))
+      {
+        LOG(ERROR) << "Not a valid shear matrix";
+        throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError);
+      }
+
+      Matrix m = IdentityMatrix(4);
+      m(0,2) = -shear(0,2);
+      m(1,2) = -shear(1,2);
+      m(3,2) = -shear(3,2);
+
+      return m;
+    }
   }
 }
--- a/Framework/Toolbox/LinearAlgebra.h	Wed Feb 14 16:49:43 2018 +0100
+++ b/Framework/Toolbox/LinearAlgebra.h	Thu Feb 15 13:51:29 2018 +0100
@@ -52,6 +52,9 @@
                      const Orthanc::DicomTag& tag);
 
     void AssignVector(Vector& v,
+                      double v1);
+
+    void AssignVector(Vector& v,
                       double v1,
                       double v2);
 
@@ -66,6 +69,20 @@
                       double v3,
                       double v4);
 
+    Vector CreateVector(double v1);
+
+    Vector CreateVector(double v1,
+                        double v2);
+
+    Vector CreateVector(double v1,
+                        double v2,
+                        double v3);
+    
+    Vector CreateVector(double v1,
+                        double v2,
+                        double v3,
+                        double v4);
+
     inline bool IsNear(double x,
                        double y,
                        double threshold)
@@ -99,6 +116,74 @@
     void Convert(Matrix& target,
                  const Vector& source);
 
+    inline Matrix Transpose(const Matrix& a)
+    {
+      return boost::numeric::ublas::trans(a);
+    }
+
+
+    inline Matrix IdentityMatrix(size_t size)
+    {
+      return boost::numeric::ublas::identity_matrix<double>(size);
+    }
+
+
+    inline Matrix ZeroMatrix(size_t size1,
+                             size_t size2)
+    {
+      return boost::numeric::ublas::zero_matrix<double>(size1, size2);
+    }
+
+
+    inline Matrix Product(const Matrix& a,
+                          const Matrix& b)
+    {
+      return boost::numeric::ublas::prod(a, b);
+    }
+
+
+    inline Vector Product(const Matrix& a,
+                          const Vector& b)
+    {
+      return boost::numeric::ublas::prod(a, b);
+    }
+
+
+    inline Matrix Product(const Matrix& a,
+                          const Matrix& b,
+                          const Matrix& c)
+    {
+      return Product(a, Product(b, c));
+    }
+
+
+    inline Matrix Product(const Matrix& a,
+                          const Matrix& b,
+                          const Matrix& c,
+                          const Matrix& d)
+    {
+      return Product(a, Product(b, c, d));
+    }
+
+
+    inline Matrix Product(const Matrix& a,
+                          const Matrix& b,
+                          const Matrix& c,
+                          const Matrix& d,
+                          const Matrix& e)
+    {
+      return Product(a, Product(b, c, d, e));
+    }
+
+
+    inline Vector Product(const Matrix& a,
+                          const Matrix& b,
+                          const Vector& c)
+    {
+      return Product(Product(a, b), c);
+    }
+
+
     double ComputeDeterminant(const Matrix& a);
 
     bool IsOrthogonalMatrix(const Matrix& q,
@@ -127,5 +212,18 @@
 
     void InvertMatrix(Matrix& target,
                       const Matrix& source);
+
+    void CreateSkewSymmetric(Matrix& s,
+                             const Vector& v);
+  
+    void AlignVectorsWithRotation(Matrix& r,
+                                  const Vector& a,
+                                  const Vector& b);
+
+    Matrix InvertScaleTranslationMatrix(const Matrix& t);
+
+    bool IsShearMatrix(const Matrix& shear);  
+
+    Matrix InvertShearMatrix(const Matrix& shear);
   };
 }
--- a/UnitTestsSources/UnitTestsMain.cpp	Wed Feb 14 16:49:43 2018 +0100
+++ b/UnitTestsSources/UnitTestsMain.cpp	Thu Feb 15 13:51:29 2018 +0100
@@ -365,10 +365,13 @@
 
   // Image plane that led to these parameters, with principal point at
   // (256,256). The image has dimensions 512x512.
-  OrthancStone::Vector o, ax, ay;
-  OrthancStone::LinearAlgebra::AssignVector(o, 7.009620, 2.521030, -821.942000);
-  OrthancStone::LinearAlgebra::AssignVector(ax, -0.453219, 0.891399, -0.001131 );
-  OrthancStone::LinearAlgebra::AssignVector(ay,  -0.891359, -0.453210, -0.008992);
+  OrthancStone::Vector o =
+    OrthancStone::LinearAlgebra::CreateVector(7.009620, 2.521030, -821.942000);
+  OrthancStone::Vector ax =
+    OrthancStone::LinearAlgebra::CreateVector(-0.453219, 0.891399, -0.001131);
+  OrthancStone::Vector ay =
+    OrthancStone::LinearAlgebra::CreateVector(-0.891359, -0.453210, -0.008992);
+
   OrthancStone::CoordinateSystem3D imagePlane(o, ax, ay);
 
   // Back-projection of the principal point