# HG changeset patch # User Sebastien Jodogne # Date 1518699089 -3600 # Node ID 8d50e6be565d67b5ab1430c32e8dc1c5f5c947c0 # Parent 432b1f812d14f0c480cf88ed1ab8c15281b287dc LinearAlgebra toolbox diff -r 432b1f812d14 -r 8d50e6be565d Framework/Toolbox/GeometryToolbox.cpp --- 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, diff -r 432b1f812d14 -r 8d50e6be565d Framework/Toolbox/GeometryToolbox.h --- 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, diff -r 432b1f812d14 -r 8d50e6be565d Framework/Toolbox/LinearAlgebra.cpp --- 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(3) + + sine * k + + (1 - cosine) * boost::numeric::ublas::prod(k, k)); +#else + r = (boost::numeric::ublas::identity_matrix(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; + } } } diff -r 432b1f812d14 -r 8d50e6be565d Framework/Toolbox/LinearAlgebra.h --- 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(size); + } + + + inline Matrix ZeroMatrix(size_t size1, + size_t size2) + { + return boost::numeric::ublas::zero_matrix(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); }; } diff -r 432b1f812d14 -r 8d50e6be565d UnitTestsSources/UnitTestsMain.cpp --- 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