Math: SVD algorithm implementation.

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)

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š <mosra@centrum.cz>
+
+    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 <tuple>
+
+#include "Math/Functions.h"
+#include "Math/Matrix.h"
+
+namespace Magnum { namespace Math { namespace Algorithms {
+
+#ifndef DOXYGEN_GENERATING_OUTPUT
+namespace Implementation {
+
+template<class T> 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<class T> T smallestDelta();
+template<> inline constexpr float smallestDelta<float>() { return 1.0e-32; }
+template<> inline constexpr double smallestDelta<double>() { 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<cols, rows, double> m;
+
+RectangularMatrix<cols, rows, double> uPart;
+Vector<cols, double> wDiagonal;
+Matrix<cols, double> v;
+
+std::tie(uPart, wDiagonal, v) = Math::Algorithms::svd(m);
+
+// Extend U
+Matrix<rows, double> u(Matrix<rows, double>::Zero);
+for(std::size_t i = 0; i != rows; ++i)
+    u[i] = uPart[i];
+
+// Diagonal W
+RectangularMatrix<cols, rows, double> w =
+    RectangularMatrix<cols, rows, double>::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::size_t cols, std::size_t rows, class T> std::tuple<RectangularMatrix<cols, rows, T>, Vector<cols, T>, Matrix<cols, T>> svd(RectangularMatrix<cols, rows, T> m) {
+    static_assert(rows >= cols, "Unsupported matrix aspect ratio");
+    static_assert(T(1)+MathTypeTraits<T>::epsilon() > T(1), "Epsilon too small");
+    constexpr T tol = Implementation::smallestDelta<T>()/MathTypeTraits<T>::epsilon();
+    static_assert(tol > T(0), "Tol too small");
+    constexpr std::size_t maxIterations = 50;
+
+    Matrix<cols, T> v(Matrix<cols, T>::Zero);
+    Vector<cols, T> 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<T>::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<rows, T> a = m[l1];
+                    const Vector<rows, T> 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<cols, rows, T>(), Vector<cols, T>(), Matrix<cols, T>(Matrix<cols, T>::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<cols, T> a1 = v[i-1];
+                const Vector<cols, T> 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<rows, T> a2 = m[i-1];
+                const Vector<rows, T> 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

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)

src/Math/Algorithms/Test/SvdTest.cpp

@@ -0,0 +1,97 @@
+/*
+    Copyright © 2010, 2011, 2012 Vladimír Vondruš <mosra@centrum.cz>
+
+    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 <TestSuite/Tester.h>
+
+#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)