Mercurial > hg > orthanc-stone
diff Framework/Toolbox/LinearAlgebra.cpp @ 159:0a73d76333db wasm
populating LinearAlgebra
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 09:15:08 +0100 |
parents | a053ca7fa5c6 |
children | e9dae7e7bffc |
line wrap: on
line diff
--- a/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 08:58:31 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 09:15:08 2018 +0100 @@ -196,5 +196,244 @@ target(i, 0) = source[i]; } } + + + double ComputeDeterminant(const Matrix& a) + { + if (a.size1() != a.size2()) + { + LOG(ERROR) << "Determinant only exists for square matrices"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + // https://en.wikipedia.org/wiki/Rule_of_Sarrus + if (a.size1() == 1) + { + return a(0,0); + } + else if (a.size1() == 2) + { + return a(0,0) * a(1,1) - a(0,1) * a(1,0); + } + else if (a.size1() == 3) + { + return (a(0,0) * a(1,1) * a(2,2) + + a(0,1) * a(1,2) * a(2,0) + + a(0,2) * a(1,0) * a(2,1) - + a(2,0) * a(1,1) * a(0,2) - + a(2,1) * a(1,2) * a(0,0) - + a(2,2) * a(1,0) * a(0,1)); + } + else + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + } + } + + + bool IsOrthogonalMatrix(const Matrix& q, + double threshold) + { + // https://en.wikipedia.org/wiki/Orthogonal_matrix + + using namespace boost::numeric::ublas; + + const Matrix check = prod(trans(q), q) - identity_matrix<double>(3); + + typename type_traits<double>::real_type norm = norm_inf(check); + + return (norm <= threshold); + } + + + bool IsOrthogonalMatrix(const Matrix& q) + { + return IsOrthogonalMatrix(q, 10.0 * std::numeric_limits<float>::epsilon()); + } + + + bool IsRotationMatrix(const Matrix& r, + double threshold) + { + return (IsOrthogonalMatrix(r, threshold) && + IsNear(ComputeDeterminant(r), 1.0, threshold)); + } + + + bool IsRotationMatrix(const Matrix& r) + { + return IsRotationMatrix(r, 10.0 * std::numeric_limits<float>::epsilon()); + } + + + void InvertUpperTriangularMatrix(Matrix& output, + const Matrix& k) + { + if (k.size1() != k.size2()) + { + LOG(ERROR) << "Determinant only exists for square matrices"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + output.resize(k.size1(), k.size2()); + + for (size_t i = 1; i < k.size1(); i++) + { + for (size_t j = 0; j < i; j++) + { + if (!IsCloseToZero(k(i, j))) + { + LOG(ERROR) << "Not an upper triangular matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + output(i, j) = 0; // The output is also upper triangular + } + } + + if (k.size1() == 3) + { + // https://math.stackexchange.com/a/1004181 + double a = k(0, 0); + double b = k(0, 1); + double c = k(0, 2); + double d = k(1, 1); + double e = k(1, 2); + double f = k(2, 2); + + if (IsCloseToZero(a) || + IsCloseToZero(d) || + IsCloseToZero(f)) + { + LOG(ERROR) << "Singular upper triangular matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + else + { + output(0, 0) = 1.0 / a; + output(0, 1) = -b / (a * d); + output(0, 2) = (b * e - c * d) / (a * f * d); + output(1, 1) = 1.0 / d; + output(1, 2) = -e / (f * d); + output(2, 2) = 1.0 / f; + } + } + else + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + } + } + + + static void GetGivensComponent(double& c, + double& s, + const Matrix& a, + size_t i, + size_t j) + { + assert(i < 3 && j < 3); + + double x = a(i, i); + double y = a(i, j); + double n = sqrt(x * x + y * y); + + if (IsCloseToZero(n)) + { + c = 1; + s = 0; + } + else + { + c = x / n; + s = -y / n; + } + } + + + /** + * This function computes the RQ decomposition of a 3x3 matrix, + * using Givens rotations. Reference: Algorithm A4.1 (page 579) of + * "Multiple View Geometry in Computer Vision" (2nd edition). The + * output matrix "Q" is a rotation matrix, and "R" is upper + * triangular. + **/ + void RQDecomposition3x3(Matrix& r, + Matrix& q, + const Matrix& a) + { + using namespace boost::numeric::ublas; + + if (a.size1() != 3 || + a.size2() != 3) + { + LOG(ERROR) << "Only applicable to a 3x3 matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + r.resize(3, 3); + q.resize(3, 3); + + r = a; + q = identity_matrix<double>(3); + + { + // Set A(2,1) to zero + double c, s; + GetGivensComponent(c, s, r, 2, 1); + + double v[9] = { 1, 0, 0, + 0, c, -s, + 0, s, c }; + + Matrix g; + FillMatrix(g, 3, 3, v); + + r = prod(r, g); + q = prod(trans(g), q); + } + + + { + // Set A(2,0) to zero + double c, s; + GetGivensComponent(c, s, r, 2, 0); + + double v[9] = { c, 0, -s, + 0, 1, 0, + s, 0, c }; + + Matrix g; + FillMatrix(g, 3, 3, v); + + r = prod(r, g); + q = prod(trans(g), q); + } + + + { + // Set A(1,0) to zero + double c, s; + GetGivensComponent(c, s, r, 1, 0); + + double v[9] = { c, -s, 0, + s, c, 0, + 0, 0, 1 }; + + Matrix g; + FillMatrix(g, 3, 3, v); + + r = prod(r, g); + q = prod(trans(g), q); + } + + if (!IsCloseToZero(norm_inf(prod(r, q) - a)) || + !IsRotationMatrix(q) || + !IsCloseToZero(r(1, 0)) || + !IsCloseToZero(r(2, 0)) || + !IsCloseToZero(r(2, 1))) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); + } + } } }