Mercurial > hg > orthanc-stone
view Framework/Toolbox/LinearAlgebra.cpp @ 270:2d64f4d39610 am-2
backup (work in progress)
author | am@osimis.io |
---|---|
date | Thu, 23 Aug 2018 14:45:04 +0200 |
parents | 5412adf19980 |
children | d87fe075d31b |
line wrap: on
line source
/** * Stone of Orthanc * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics * Department, University Hospital of Liege, Belgium * Copyright (C) 2017-2018 Osimis S.A., Belgium * * This program is free software: you can redistribute it and/or * modify it under the terms of the GNU Affero General Public License * as published by the Free Software Foundation, either version 3 of * the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * Affero General Public License for more details. * * You should have received a copy of the GNU Affero General Public License * along with this program. If not, see <http://www.gnu.org/licenses/>. **/ #include "LinearAlgebra.h" #include <Core/Logging.h> #include <Core/OrthancException.h> #include <Core/Toolbox.h> #include <stdio.h> #include <boost/lexical_cast.hpp> #include <boost/numeric/ublas/lu.hpp> namespace OrthancStone { namespace LinearAlgebra { void Print(const Vector& v) { for (size_t i = 0; i < v.size(); i++) { printf("%g\n", v[i]); //printf("%8.2f\n", v[i]); } printf("\n"); } void Print(const Matrix& m) { for (size_t i = 0; i < m.size1(); i++) { for (size_t j = 0; j < m.size2(); j++) { printf("%g ", m(i,j)); //printf("%8.2f ", m(i,j)); } printf("\n"); } printf("\n"); } bool ParseVector(Vector& target, const std::string& value) { std::vector<std::string> items; Orthanc::Toolbox::TokenizeString(items, value, '\\'); target.resize(items.size()); for (size_t i = 0; i < items.size(); i++) { try { target[i] = boost::lexical_cast<double>(Orthanc::Toolbox::StripSpaces(items[i])); } catch (boost::bad_lexical_cast&) { target.clear(); return false; } } return true; } bool ParseVector(Vector& target, const Orthanc::DicomMap& dataset, const Orthanc::DicomTag& tag) { std::string value; return (dataset.CopyToString(value, tag, false) && ParseVector(target, value)); } void NormalizeVector(Vector& u) { double norm = boost::numeric::ublas::norm_2(u); if (!IsCloseToZero(norm)) { u = u / norm; } } void CrossProduct(Vector& result, const Vector& u, const Vector& v) { if (u.size() != 3 || v.size() != 3) { throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); } result.resize(3); result[0] = u[1] * v[2] - u[2] * v[1]; result[1] = u[2] * v[0] - u[0] * v[2]; result[2] = u[0] * v[1] - u[1] * v[0]; } void FillMatrix(Matrix& target, size_t rows, size_t columns, const double values[]) { target.resize(rows, columns); size_t index = 0; for (size_t y = 0; y < rows; y++) { for (size_t x = 0; x < columns; x++, index++) { target(y, x) = values[index]; } } } void FillVector(Vector& target, size_t size, const double values[]) { target.resize(size); for (size_t i = 0; i < size; i++) { target[i] = values[i]; } } void Convert(Matrix& target, const Vector& source) { const size_t n = source.size(); target.resize(n, 1); for (size_t i = 0; i < n; i++) { 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); 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); } } bool InvertMatrixUnsafe(Matrix& target, const Matrix& source) { if (source.size1() != source.size2()) { LOG(ERROR) << "Inverse only exists for square matrices"; throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); } if (source.size1() < 4) { // 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 true; } double determinant = ComputeDeterminant(source); if (IsCloseToZero(determinant)) { return false; } double denominator = 1.0 / determinant; target.resize(source.size1(), source.size2()); if (source.size1() == 1) { target(0, 0) = denominator; return true; } 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; return true; } 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; return true; } else { throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); } } else { // 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) { return false; } else { target = boost::numeric::ublas::identity_matrix<double>(source.size1()); lu_substitute(a, permutation, target); return true; } } } void InvertMatrix(Matrix& target, const Matrix& source) { if (!InvertMatrixUnsafe(target, source)) { LOG(ERROR) << "Cannot invert singular matrix"; throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); } } 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; } Matrix InvertScalingTranslationMatrix(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; } } }