ImplicitQRSVD.h

Go to the documentation of this file.

 
#ifndef JIXIE_IMPLICIT_QR_SVD_H
#define JIXIE_IMPLICIT_QR_SVD_H
 
#include "Tools.h"
 
namespace JIXIE {
 
template <class T>
class GivensRotation {
public:
    int rowi;
    int rowk;
    T c;
    T s;
 
    inline GivensRotation(int rowi_in, int rowk_in)
        : rowi(rowi_in)
        , rowk(rowk_in)
        , c(1)
        , s(0)
    {
    }
 
    inline GivensRotation(T a, T b, int rowi_in, int rowk_in)
        : rowi(rowi_in)
        , rowk(rowk_in)
    {
        compute(a, b);
    }
 
    ~GivensRotation() {}
 
    inline void transposeInPlace()
    {
        s = -s;
    }
 
    inline void compute(const T a, const T b)
    {
        using std::sqrt;
 
        T d = a * a + b * b;
        c = 1;
        s = 0;
        if (d != 0) {
            // T t = 1 / sqrt(d);
            T t = JIXIE::MATH_TOOLS::rsqrt(d);
            c = a * t;
            s = -b * t;
        }
    }
 
    inline void computeUnconventional(const T a, const T b)
    {
        using std::sqrt;
 
        T d = a * a + b * b;
        c = 0;
        s = 1;
        if (d != 0) {
            // T t = 1 / sqrt(d);
            T t = JIXIE::MATH_TOOLS::rsqrt(d);
            s = a * t;
            c = b * t;
        }
    }
    template <class MatrixType>
    inline void fill(const MatrixType& R) const
    {
        MatrixType& A = const_cast<MatrixType&>(R);
        A = MatrixType::Identity();
        A(rowi, rowi) = c;
        A(rowk, rowi) = -s;
        A(rowi, rowk) = s;
        A(rowk, rowk) = c;
    }
 
    template <class MatrixType>
    inline void rowRotation(MatrixType& A) const
    {
        for (int j = 0; j < MatrixType::ColsAtCompileTime; j++) {
            T tau1 = A(rowi, j);
            T tau2 = A(rowk, j);
            A(rowi, j) = c * tau1 - s * tau2;
            A(rowk, j) = s * tau1 + c * tau2;
        }
    }
 
    template <class MatrixType>
    inline void columnRotation(MatrixType& A) const
    {
        for (int j = 0; j < MatrixType::RowsAtCompileTime; j++) {
            T tau1 = A(j, rowi);
            T tau2 = A(j, rowk);
            A(j, rowi) = c * tau1 - s * tau2;
            A(j, rowk) = s * tau1 + c * tau2;
        }
    }
 
    inline void operator*=(const GivensRotation<T>& A)
    {
        T new_c = c * A.c - s * A.s;
        T new_s = s * A.c + c * A.s;
        c = new_c;
        s = new_s;
    }
 
    inline GivensRotation<T> operator*(const GivensRotation<T>& A) const
    {
        GivensRotation<T> r(*this);
        r *= A;
        return r;
    }
};
 
template <class T>
inline void zeroChase(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
{
 
    GivensRotation<T> r1(H(0, 0), H(1, 0), 0, 1);
    GivensRotation<T> r2(1, 2);
    if (H(1, 0) != 0)
        r2.compute(H(0, 0) * H(0, 1) + H(1, 0) * H(1, 1), H(0, 0) * H(0, 2) + H(1, 0) * H(1, 2));
    else
        r2.compute(H(0, 1), H(0, 2));
 
    r1.rowRotation(H);
 
    /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
    r2.columnRotation(H);
    r2.columnRotation(V);
 
    GivensRotation<T> r3(H(1, 1), H(2, 1), 1, 2);
    r3.rowRotation(H);
 
    // Save this till end for better cache coherency
    // r1.rowRotation(u_transpose);
    // r3.rowRotation(u_transpose);
    r1.columnRotation(U);
    r3.columnRotation(U);
}
 
template <class T>
inline void makeUpperBidiag(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
{
    U = Eigen::Matrix<T, 3, 3>::Identity();
    V = Eigen::Matrix<T, 3, 3>::Identity();
 
    GivensRotation<T> r(H(1, 0), H(2, 0), 1, 2);
    r.rowRotation(H);
    // r.rowRotation(u_transpose);
    r.columnRotation(U);
    // zeroChase(H, u_transpose, V);
    zeroChase(H, U, V);
}
 
template <class T>
inline void makeLambdaShape(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
{
    U = Eigen::Matrix<T, 3, 3>::Identity();
    V = Eigen::Matrix<T, 3, 3>::Identity();
 
    GivensRotation<T> r1(H(0, 1), H(0, 2), 1, 2);
    r1.columnRotation(H);
    r1.columnRotation(V);
 
    r1.computeUnconventional(H(1, 2), H(2, 2));
    r1.rowRotation(H);
    r1.columnRotation(U);
 
    GivensRotation<T> r2(H(2, 0), H(2, 1), 0, 1);
    r2.columnRotation(H);
    r2.columnRotation(V);
 
    r2.computeUnconventional(H(0, 1), H(1, 1));
    r2.rowRotation(H);
    r2.columnRotation(U);
}
 
template <class TA, class T, class TS>
inline enable_if_t<isSize<TA>(2, 2) && isSize<TS>(2, 2)>
polarDecomposition(const Eigen::MatrixBase<TA>& A,
    GivensRotation<T>& R,
    const Eigen::MatrixBase<TS>& S_Sym)
{
    Eigen::Matrix<T, 2, 1> x(A(0, 0) + A(1, 1), A(1, 0) - A(0, 1));
    T denominator = x.norm();
    R.c = (T)1;
    R.s = (T)0;
    if (denominator != 0) {
        /*
          No need to use a tolerance here because x(0) and x(1) always have
          smaller magnitude then denominator, therefore overflow never happens.
        */
        R.c = x(0) / denominator;
        R.s = -x(1) / denominator;
    }
    Eigen::MatrixBase<TS>& S = const_cast<Eigen::MatrixBase<TS>&>(S_Sym);
    S = A;
    R.rowRotation(S);
}
 
template <class TA, class TR, class TS>
inline enable_if_t<isSize<TA>(2, 2) && isSize<TR>(2, 2) && isSize<TS>(2, 2)>
polarDecomposition(const Eigen::MatrixBase<TA>& A,
    const Eigen::MatrixBase<TR>& R,
    const Eigen::MatrixBase<TS>& S_Sym)
{
    using T = ScalarType<TA>;
    GivensRotation<T> r(0, 1);
    polarDecomposition(A, r, S_Sym);
    r.fill(R);
}
 
template <class TA, class T, class Ts>
inline enable_if_t<isSize<TA>(2, 2) && isSize<Ts>(2, 1)>
singularValueDecomposition(
    const Eigen::MatrixBase<TA>& A,
    GivensRotation<T>& U,
    const Eigen::MatrixBase<Ts>& Sigma,
    GivensRotation<T>& V,
    const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA>>::epsilon())
{
    using std::sqrt;
    Eigen::MatrixBase<Ts>& sigma = const_cast<Eigen::MatrixBase<Ts>&>(Sigma);
 
    Eigen::Matrix<T, 2, 2> S_Sym;
    polarDecomposition(A, U, S_Sym);
    T cosine, sine;
    T x = S_Sym(0, 0);
    T y = S_Sym(0, 1);
    T z = S_Sym(1, 1);
    if (y == 0) {
        // S is already diagonal
        cosine = 1;
        sine = 0;
        sigma(0) = x;
        sigma(1) = z;
    }
    else {
        T tau = 0.5 * (x - z);
        T w = sqrt(tau * tau + y * y);
        // w > y > 0
        T t;
        if (tau > 0) {
            // tau + w > w > y > 0 ==> division is safe
            t = y / (tau + w);
        }
        else {
            // tau - w < -w < -y < 0 ==> division is safe
            t = y / (tau - w);
        }
        cosine = T(1) / sqrt(t * t + T(1));
        sine = -t * cosine;
        /*
          V = [cosine -sine; sine cosine]
          Sigma = V'SV. Only compute the diagonals for efficiency.
          Also utilize symmetry of S and don't form V yet.
        */
        T c2 = cosine * cosine;
        T csy = 2 * cosine * sine * y;
        T s2 = sine * sine;
        sigma(0) = c2 * x - csy + s2 * z;
        sigma(1) = s2 * x + csy + c2 * z;
    }
 
    // Sorting
    // Polar already guarantees negative sign is on the small magnitude singular value.
    if (sigma(0) < sigma(1)) {
        std::swap(sigma(0), sigma(1));
        V.c = -sine;
        V.s = cosine;
    }
    else {
        V.c = cosine;
        V.s = sine;
    }
    U *= V;
}
template <class TA, class TU, class Ts, class TV>
inline enable_if_t<isSize<TA>(2, 2) && isSize<TU>(2, 2) && isSize<TV>(2, 2) && isSize<Ts>(2, 1)>
singularValueDecomposition(
    const Eigen::MatrixBase<TA>& A,
    const Eigen::MatrixBase<TU>& U,
    const Eigen::MatrixBase<Ts>& Sigma,
    const Eigen::MatrixBase<TV>& V,
    const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA>>::epsilon())
{
    using T = ScalarType<TA>;
    GivensRotation<T> gv(0, 1);
    GivensRotation<T> gu(0, 1);
    singularValueDecomposition(A, gu, Sigma, gv);
 
    gu.fill(U);
    gv.fill(V);
}
 
template <class T>
T wilkinsonShift(const T a1, const T b1, const T a2)
{
    using std::copysign;
    using std::fabs;
    using std::sqrt;
 
    T d = (T)0.5 * (a1 - a2);
    T bs = b1 * b1;
 
    T mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
    // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
    return mu;
}
 
template <int t, class T>
inline void process(Eigen::Matrix<T, 3, 3>& B, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
{
    int other = (t == 1) ? 0 : 2;
    GivensRotation<T> u(0, 1);
    GivensRotation<T> v(0, 1);
    sigma(other) = B(other, other);
    singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
    u.rowi += t;
    u.rowk += t;
    v.rowi += t;
    v.rowk += t;
    u.columnRotation(U);
    v.columnRotation(V);
}
 
template <class T>
inline void flipSign(int i, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma)
{
    sigma(i) = -sigma(i);
    U.col(i) = -U.col(i);
}
 
template <int t, class T>
enable_if_t<t == 0> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
{
    using std::fabs;
 
    // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
    if (fabs(sigma(1)) >= fabs(sigma(2))) {
        if (sigma(1) < 0) {
            flipSign(1, U, sigma);
            flipSign(2, U, sigma);
        }
        return;
    }
 
    //fix sign of sigma for both cases
    if (sigma(2) < 0) {
        flipSign(1, U, sigma);
        flipSign(2, U, sigma);
    }
 
    //swap sigma(1) and sigma(2) for both cases
    std::swap(sigma(1), sigma(2));
    U.col(1).swap(U.col(2));
    V.col(1).swap(V.col(2));
 
    // Case: |sigma(2)| >= sigma(0) > |simga(1)|
    if (sigma(1) > sigma(0)) {
        std::swap(sigma(0), sigma(1));
        U.col(0).swap(U.col(1));
        V.col(0).swap(V.col(1));
    }
 
    // Case: sigma(0) >= |sigma(2)| > |simga(1)|
    else {
        U.col(2) = -U.col(2);
        V.col(2) = -V.col(2);
    }
}
 
template <int t, class T>
enable_if_t<t == 1> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
{
    using std::fabs;
 
    // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
    if (fabs(sigma(0)) >= sigma(1)) {
        if (sigma(0) < 0) {
            flipSign(0, U, sigma);
            flipSign(2, U, sigma);
        }
        return;
    }
 
    //swap sigma(0) and sigma(1) for both cases
    std::swap(sigma(0), sigma(1));
    U.col(0).swap(U.col(1));
    V.col(0).swap(V.col(1));
 
    // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
    if (fabs(sigma(1)) < fabs(sigma(2))) {
        std::swap(sigma(1), sigma(2));
        U.col(1).swap(U.col(2));
        V.col(1).swap(V.col(2));
    }
 
    // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
    else {
        U.col(1) = -U.col(1);
        V.col(1) = -V.col(1);
    }
 
    // fix sign for both cases
    if (sigma(1) < 0) {
        flipSign(1, U, sigma);
        flipSign(2, U, sigma);
    }
}
 
template <class T>
inline int singularValueDecomposition(const Eigen::Matrix<T, 3, 3>& A,
    Eigen::Matrix<T, 3, 3>& U,
    Eigen::Matrix<T, 3, 1>& sigma,
    Eigen::Matrix<T, 3, 3>& V,
    T tol = 1024 * std::numeric_limits<T>::epsilon())
{
    using std::fabs;
    using std::max;
    using std::sqrt;
    Eigen::Matrix<T, 3, 3> B = A;
    U = Eigen::Matrix<T, 3, 3>::Identity();
    V = Eigen::Matrix<T, 3, 3>::Identity();
 
    makeUpperBidiag(B, U, V);
 
    int count = 0;
    T mu = (T)0;
    GivensRotation<T> r(0, 1);
 
    T alpha_1 = B(0, 0);
    T beta_1 = B(0, 1);
    T alpha_2 = B(1, 1);
    T alpha_3 = B(2, 2);
    T beta_2 = B(1, 2);
    T gamma_1 = alpha_1 * beta_1;
    T gamma_2 = alpha_2 * beta_2;
    tol *= max((T)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (T)1);
 
    while (fabs(beta_2) > tol && fabs(beta_1) > tol
        && fabs(alpha_1) > tol && fabs(alpha_2) > tol
        && fabs(alpha_3) > tol) {
        mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
 
        r.compute(alpha_1 * alpha_1 - mu, gamma_1);
        r.columnRotation(B);
 
        r.columnRotation(V);
        zeroChase(B, U, V);
 
        alpha_1 = B(0, 0);
        beta_1 = B(0, 1);
        alpha_2 = B(1, 1);
        alpha_3 = B(2, 2);
        beta_2 = B(1, 2);
        gamma_1 = alpha_1 * beta_1;
        gamma_2 = alpha_2 * beta_2;
        count++;
    }
    if (fabs(beta_2) <= tol) {
        process<0>(B, U, sigma, V);
        sort<0>(U, sigma, V);
    }
    else if (fabs(beta_1) <= tol) {
        process<1>(B, U, sigma, V);
        sort<1>(U, sigma, V);
    }
    else if (fabs(alpha_2) <= tol) {
        GivensRotation<T> r1(1, 2);
        r1.computeUnconventional(B(1, 2), B(2, 2));
        r1.rowRotation(B);
        r1.columnRotation(U);
 
        process<0>(B, U, sigma, V);
        sort<0>(U, sigma, V);
    }
    else if (fabs(alpha_3) <= tol) {
        GivensRotation<T> r1(1, 2);
        r1.compute(B(1, 1), B(1, 2));
        r1.columnRotation(B);
        r1.columnRotation(V);
        GivensRotation<T> r2(0, 2);
        r2.compute(B(0, 0), B(0, 2));
        r2.columnRotation(B);
        r2.columnRotation(V);
 
        process<0>(B, U, sigma, V);
        sort<0>(U, sigma, V);
    }
    else if (fabs(alpha_1) <= tol) {
        GivensRotation<T> r1(0, 1);
        r1.computeUnconventional(B(0, 1), B(1, 1));
        r1.rowRotation(B);
        r1.columnRotation(U);
 
        GivensRotation<T> r2(0, 2);
        r2.computeUnconventional(B(0, 2), B(2, 2));
        r2.rowRotation(B);
        r2.columnRotation(U);
 
        process<1>(B, U, sigma, V);
        sort<1>(U, sigma, V);
    }
 
    return count;
}
 
template <class T>
inline void polarDecomposition(const Eigen::Matrix<T, 3, 3>& A,
    Eigen::Matrix<T, 3, 3>& R,
    Eigen::Matrix<T, 3, 3>& S_Sym)
{
    Eigen::Matrix<T, 3, 3> U;
    Eigen::Matrix<T, 3, 1> sigma;
    Eigen::Matrix<T, 3, 3> V;
 
    singularValueDecomposition(A, U, sigma, V);
    R.noalias() = U * V.transpose();
    S_Sym.noalias() = V * Eigen::DiagonalMatrix<T, 3, 3>(sigma) * V.transpose();
}
} // namespace JIXIE
#endif