Mercurial > hg > orthanc-stone
diff Framework/Toolbox/LinearAlgebra.cpp @ 165:8d50e6be565d wasm
LinearAlgebra toolbox
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Thu, 15 Feb 2018 13:51:29 +0100 |
parents | 432b1f812d14 |
children | 4f661e2f7b6c |
line wrap: on
line diff
--- 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; + } } }