From 2d0aad34b2980c84647ac247bbd74ddca652b9f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vladim=C3=ADr=20Vondru=C5=A1?= Date: Wed, 30 Jan 2013 14:51:25 +0100 Subject: [PATCH] Math: SVD algorithm implementation. --- src/Math/Algorithms/CMakeLists.txt | 3 +- src/Math/Algorithms/Svd.h | 328 ++++++++++++++++++++++++ src/Math/Algorithms/Test/CMakeLists.txt | 1 + src/Math/Algorithms/Test/SvdTest.cpp | 97 +++++++ 4 files changed, 428 insertions(+), 1 deletion(-) create mode 100644 src/Math/Algorithms/Svd.h create mode 100644 src/Math/Algorithms/Test/SvdTest.cpp diff --git a/src/Math/Algorithms/CMakeLists.txt b/src/Math/Algorithms/CMakeLists.txt index 0a54d2d3c..66cf6c7e9 100644 --- a/src/Math/Algorithms/CMakeLists.txt +++ b/src/Math/Algorithms/CMakeLists.txt @@ -1,6 +1,7 @@ set(MagnumMathAlgorithms_HEADERS GaussJordan.h - GramSchmidt.h) + GramSchmidt.h + Svd.h) install(FILES ${MagnumMathAlgorithms_HEADERS} DESTINATION ${MAGNUM_INCLUDE_INSTALL_DIR}/Math/Algorithms) if(BUILD_TESTS) diff --git a/src/Math/Algorithms/Svd.h b/src/Math/Algorithms/Svd.h new file mode 100644 index 000000000..7320c513c --- /dev/null +++ b/src/Math/Algorithms/Svd.h @@ -0,0 +1,328 @@ +#ifndef Magnum_Math_Algorithms_Svd_h +#define Magnum_Math_Algorithms_Svd_h +/* + Copyright © 2010, 2011, 2012 Vladimír Vondruš + + This file is part of Magnum. + + Magnum is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 3 + only, as published by the Free Software Foundation. + + Magnum 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 Lesser General Public License version 3 for more details. +*/ + +/** @file + * @brief Function Magnum::Math::Algorithms::svd() + */ + +#include + +#include "Math/Functions.h" +#include "Math/Matrix.h" + +namespace Magnum { namespace Math { namespace Algorithms { + +#ifndef DOXYGEN_GENERATING_OUTPUT +namespace Implementation { + +template T pythagoras(T a, T b) { + T absa = std::abs(a); + T absb = std::abs(b); + if(absa > absb) + return absa*std::sqrt(T(1) + Math::pow<2>(absb/absa)); + else if(absb == T(0)) /** @todo epsilon comparison? */ + return T(0); + else + return absb*std::sqrt(T(1) + Math::pow<2>(absa/absb)); +} + +template T smallestDelta(); +template<> inline constexpr float smallestDelta() { return 1.0e-32; } +template<> inline constexpr double smallestDelta() { return 1.0e-64; } + +} +#endif + +/** +@brief Singular Value Decomposition + +Performs Thin SVD on given matrix where @p rows >= @p cols: +@f[ + M = U \Sigma V^* +@f] +Returns first @p cols column vectors of @f$ U @f$, diagonal of @f$ \Sigma @f$ +and non-transposed @f$ V @f$. If the solution doesn't converge, returns +zero matrices. + +Full @f$ U @f$, @f$ \Sigma @f$ matrices and original @f$ M @f$ matrix can be +reconstructed from the values as following: +@code +RectangularMatrix m; + +RectangularMatrix uPart; +Vector wDiagonal; +Matrix v; + +std::tie(uPart, wDiagonal, v) = Math::Algorithms::svd(m); + +// Extend U +Matrix u(Matrix::Zero); +for(std::size_t i = 0; i != rows; ++i) + u[i] = uPart[i]; + +// Diagonal W +RectangularMatrix w = + RectangularMatrix::fromDiagonal(wDiagonal); + +// u*w*v.transposed() == m +@endcode + +Implementation based on *Golub, G. H.; Reinsch, C. (1970). "Singular value +decomposition and least squares solutions"*. +*/ +/* The matrix is passed by value because it is changed inside */ +template std::tuple, Vector, Matrix> svd(RectangularMatrix m) { + static_assert(rows >= cols, "Unsupported matrix aspect ratio"); + static_assert(T(1)+MathTypeTraits::epsilon() > T(1), "Epsilon too small"); + constexpr T tol = Implementation::smallestDelta()/MathTypeTraits::epsilon(); + static_assert(tol > T(0), "Tol too small"); + constexpr std::size_t maxIterations = 50; + + Matrix v(Matrix::Zero); + Vector e, q; + + /* Householder's reduction to bidiagonal form */ + T g = T(0); + T epsilonX = T(0); + for(std::size_t i = 0; i != cols; ++i) { + const std::size_t l = i+1; + + e[i] = g; + T s1 = T(0); + for(std::size_t j = i; j != rows; ++j) + s1 += Math::pow<2>(m[i][j]); + if(s1 > tol) { + const T f = m[i][i]; + g = (f < T(0) ? std::sqrt(s1) : -std::sqrt(s1)); + const T h = f*g - s1; + m[i][i] = f - g; + + for(std::size_t j = l; j != cols; ++j) { + T s = T(0); + for(std::size_t k = i; k != rows; ++k) + s += m[i][k]*m[j][k]; + const T f = s/h; + for(std::size_t k = i; k != rows; ++k) + m[j][k] += f*m[i][k]; + } + } else g = T(0); + + q[i] = g; + T s2 = T(0); + for(std::size_t j = l; j != cols; ++j) + s2 += Math::pow<2>(m[j][i]); + if(s2 > tol) { + const T f = m[i+1][i]; + g = (f < T(0) ? std::sqrt(s2) : -std::sqrt(s2)); + const T h = f*g - s2; + m[i+1][i] = f - g; + + for(std::size_t j = l; j != cols; ++j) + e[j] = m[j][i]/h; + + for(std::size_t j = l; j != rows; ++j) { + T s = T(0); + for(std::size_t k = l; k != cols; ++k) + s += m[k][j]*m[k][i]; + for(std::size_t k = l; k != cols; ++k) + m[k][j] += s*e[k]; + } + + } else g = T(0); + + const T y = std::abs(q[i]) + std::abs(e[i]); + if(y > epsilonX) epsilonX = y; + } + + /* Accumulation of right hand transformations */ + for(std::size_t l = cols; l != 0; --l) { + const std::size_t i = l-1; + + if(g != T(0)) { /** @todo epsilon check? */ + const T h = g*m[i+1][i]; + + for(std::size_t j = l; j != cols; ++j) + v[i][j] = m[j][i]/h; + + for(std::size_t j = l; j != cols; ++j) { + T s = T(0); + for(std::size_t k = l; k != cols; ++k) + s += m[k][i]*v[j][k]; + for(std::size_t k = l; k != cols; ++k) + v[j][k] += s*v[i][k]; + } + } + + for(std::size_t j = l; j != cols; ++j) + v[j][i] = v[i][j] = T(0); + + v[i][i] = T(1); + g = e[i]; + } + + /* Accumulation of left hand transformations */ + for(std::size_t l = cols; l != 0; --l) { + const std::size_t i = l-1; + + for(std::size_t j = l; j != cols; ++j) + m[j][i] = T(0); + + const T d = q[i]; + if(d != T(0)) { /** @todo epsilon check? */ + const T h = m[i][i]*d; + for(std::size_t j = l; j != cols; ++j) { + T s = T(0); + for(std::size_t k = l; k != rows; ++k) + s += m[i][k]*m[j][k]; + const T f = s/h; + for(std::size_t k = i; k != rows; ++k) + m[j][k] += f*m[i][k]; + } + + for(std::size_t j = i; j != rows; ++j) + m[i][j] /= d; + + } else for(std::size_t j = i; j != rows; ++j) + m[i][j] = T(0); + + m[i][i] += T(1); + } + + /* Diagonalization of the bidiagonal form */ + const T epsilon = MathTypeTraits::epsilon()*epsilonX; + for(std::size_t k2 = cols; k2 != 0; --k2) { + const std::size_t k = k2 - 1; + + for(std::size_t iteration = 0; iteration != maxIterations; ++iteration) { + /* Test for splitting */ + bool doCancellation = true; + std::size_t l = 0; + for(std::size_t l2 = k2; l2 != 0; --l2) { + l = l2 - 1; + if(std::abs(e[l]) <= epsilon) { + doCancellation = false; + break; + } else if(std::abs(q[l-1]) <= epsilon) { + break; + } + } + + /* Cancellation */ + if(doCancellation) { + const std::size_t l1 = l - 1; + T c = T(0); + T s = T(1); + for(std::size_t i = l; i != k+1; ++i) { + CORRADE_INTERNAL_ASSERT(i <= k+1); + + const T f = s*e[i]; + e[i] = c*e[i]; + if(std::abs(f) <= epsilon) break; + + const T g = q[i]; + const T h = Implementation::pythagoras(f, g); + q[i] = h; + c = g/h; + s = -f/h; + + const Vector a = m[l1]; + const Vector b = m[i]; + m[l1] = a*c+b*s; + m[i] = -a*s+b*c; + } + } + + /* Test for convergence */ + const T z = q[k]; + if(l == k) { + /* Invert to non-negative */ + if(z < T(0)) { + q[k] = -z; + v[k] = -v[k]; + } + + break; + + /* Exceeded iteration count, done */ + } else if(iteration >= maxIterations-1) { + Corrade::Utility::Error() << "Magnum::Math::Algorithms::svd(): no convergence"; + return std::make_tuple(RectangularMatrix(), Vector(), Matrix(Matrix::Zero)); + } + + /* Shift from bottom 2x2 minor */ + const T y = q[k-1]; + const T h = e[k]; + const T d = e[k-1]; + T x = q[l]; + T f = ((y - z)*(y + z) + (d - h)*(d + h))/(T(2)*h*y); + const T b = Implementation::pythagoras(f, T(1)); + if(f < T(0)) + f = ((x - z)*(x + z) + h*(y/(f - b) - h))/x; + else + f = ((x - z)*(x + z) + h*(y/(f + b) - h))/x; + + /* Next QR transformation */ + /** @todo isn't this extractable elsewhere? */ + T c = T(1); + T s = T(1); + for(std::size_t i = l+1; i != k+1; ++i) { + CORRADE_INTERNAL_ASSERT(i <= k+1); + + const T g1 = c*e[i]; + const T h1 = s*e[i]; + const T y1 = q[i]; + + const T z1 = Implementation::pythagoras(f, h1); + e[i-1] = z1; + c = f/z1; + s = h1/z1; + f = x*c + g1*s; + + const T g2 = -x*s + g1*c; + const T h2 = y1*s; + const T y2 = y1*c; + + const Vector a1 = v[i-1]; + const Vector b1 = v[i]; + v[i-1] = a1*c+b1*s; + v[i] = -a1*s+b1*c; + + const T z2 = Implementation::pythagoras(f, h2); + q[i-1] = z2; + c = f/z2; + s = h2/z2; + f = c*g2 + s*y2; + x = -s*g2 + c*y2; + + const Vector a2 = m[i-1]; + const Vector b2 = m[i]; + m[i-1] = a2*c+b2*s; + m[i] = -a2*s+b2*c; + } + + e[l] = T(0); + e[k] = f; + q[k] = x; + } + } + + return std::make_tuple(m, q, v); +} + +}}} + +#endif diff --git a/src/Math/Algorithms/Test/CMakeLists.txt b/src/Math/Algorithms/Test/CMakeLists.txt index e7e6975e0..15166b9b7 100644 --- a/src/Math/Algorithms/Test/CMakeLists.txt +++ b/src/Math/Algorithms/Test/CMakeLists.txt @@ -1,2 +1,3 @@ corrade_add_test(MathAlgorithmsGaussJordanTest GaussJordanTest.cpp LIBRARIES MagnumMathTestLib) corrade_add_test(MathAlgorithmsGramSchmidtTest GramSchmidtTest.cpp LIBRARIES MagnumMathTestLib) +corrade_add_test(MathAlgorithmsSvdTest SvdTest.cpp LIBRARIES MagnumMathTestLib) diff --git a/src/Math/Algorithms/Test/SvdTest.cpp b/src/Math/Algorithms/Test/SvdTest.cpp new file mode 100644 index 000000000..9c0a55560 --- /dev/null +++ b/src/Math/Algorithms/Test/SvdTest.cpp @@ -0,0 +1,97 @@ +/* + Copyright © 2010, 2011, 2012 Vladimír Vondruš + + This file is part of Magnum. + + Magnum is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License version 3 + only, as published by the Free Software Foundation. + + Magnum 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 Lesser General Public License version 3 for more details. +*/ + +#include + +#include "Math/Algorithms/Svd.h" + +namespace Magnum { namespace Math { namespace Algorithms { namespace Test { + +class SvdTest: public Corrade::TestSuite::Tester { + public: + explicit SvdTest(); + + void testDouble(); + void testFloat(); +}; + +typedef RectangularMatrix<5, 8, double> Matrix5x8d; +typedef Matrix<8, double> Matrix8d; +typedef Matrix<5, double> Matrix5d; +typedef Vector<8, double> Vector8d; +typedef Vector<5, double> Vector5d; + +typedef RectangularMatrix<5, 8, float> Matrix5x8f; +typedef Matrix<8, float> Matrix8f; +typedef Matrix<5, float> Matrix5f; +typedef Vector<8, float> Vector8f; +typedef Vector<5, float> Vector5f; + +constexpr static Matrix5x8d a( + Vector8d(22.0, 14.0, -1.0, -3.0, 9.0, 9.0, 2.0, 4.0), + Vector8d(10.0, 7.0, 13.0, -2.0, 8.0, 1.0, -6.0, 5.0), + Vector8d( 2.0, 10.0, -1.0, 13.0, 1.0, -7.0, 6.0, 0.0), + Vector8d( 3.0, 0.0, -11.0, -2.0, -2.0, 5.0, 5.0, -2.0), + Vector8d( 7.0, 8.0, 3.0, 4.0, 4.0, -1.0, 1.0, 2.0) +); + +static const Vector5d expected(std::sqrt(1248.0), 0.0, 20.0, std::sqrt(384.0), 0.0); + +SvdTest::SvdTest() { + addTests(&SvdTest::testDouble, + &SvdTest::testFloat); +} + +void SvdTest::testDouble() { + Matrix5x8d u; + Vector5d w; + Matrix5d v; + std::tie(u, w, v) = Algorithms::svd(a); + + /* Test composition */ + Matrix8d u2(u[0], u[1], u[2], u[3], u[4], Vector8d(), Vector8d(), Vector8d()); + Matrix5x8d w2 = Matrix5x8d::fromDiagonal(w); + CORRADE_COMPARE(u2*w2*v.transposed(), a); + + /* Test that V is unitary */ + CORRADE_COMPARE(v*v.transposed(), Matrix5d(Matrix5d::Identity)); + CORRADE_COMPARE(v.transposed()*v, Matrix5d(Matrix5d::Identity)); + + /* Test W */ + CORRADE_COMPARE(w, expected); +} + +void SvdTest::testFloat() { + Matrix5x8f u; + Vector5f w; + Matrix5f v; + std::tie(u, w, v) = Algorithms::svd(Matrix5x8f(a)); + + /* Test composition (single precision is not enough, test for similarity) */ + Matrix8f u2(u[0], u[1], u[2], u[3], u[4], Vector8f(), Vector8f(), Vector8f()); + Matrix5x8f w2 = Matrix5x8f::fromDiagonal(w); + CORRADE_VERIFY((u2*w2*v.transposed()-Matrix5x8f(a)).maxAbs() < 1.0e-5f); + + /* Test that V is unitary */ + CORRADE_COMPARE(v*v.transposed(), Matrix5f(Matrix5f::Identity)); + CORRADE_COMPARE(v.transposed()*v, Matrix5f(Matrix5f::Identity)); + + /* Test W (single precision is not enough, test for similarity) */ + CORRADE_VERIFY((w-Vector5f(expected)).maxAbs() < 1.0e-5f); +} + +}}}} + +CORRADE_TEST_MAIN(Magnum::Math::Algorithms::Test::SvdTest)