Cholesky.hpp

Go to the documentation of this file.

#ifndef SPIDAR_CHOLESKY_HPP
#define SPIDAR_CHOLESKY_HPP

#include <cmath>

namespace Spidar
{
namespace Math
{
class Cholesky
{
public:
    template <class MatrixType>
    static void decompose(MatrixType& mat);

    template <class MatrixType, class VectorType>
    static void solve(MatrixType& mat, VectorType& vec);

}; // end of class Cholesky.

template <class MatrixType>
void Cholesky::decompose(MatrixType& mat)
{
    static_assert(MatrixType::row==MatrixType::column, "row != column");

    const size_t size = MatrixType::row;

    for(size_t j=0; j<size; ++j)
    {
        // 対角成分
        for (size_t k=0; k<j; ++k)
        {
            mat[j][j] -= mat[j][k] * mat[j][k]; 
        }
        mat[j][j] = std::sqrt(mat[j][j]);

        // 対角成分以外のUは0
        for (size_t i=0; i<j; ++i)
        {
            mat[i][j] = 0;
        }

        // 残りのL
        for (size_t i=j+1; i<size; ++i)
        {
            for (size_t k=0; k<j; ++k)
            {
                mat[i][j] -= mat[i][k] * mat[j][k]; 
            }
            mat[i][j] /= mat[j][j];
        }
    }
}

template <class MatrixType, class VectorType>
void Cholesky::solve(MatrixType& mat, VectorType& vec)
{
    static_assert(MatrixType::row==MatrixType::column, "row != column");
    static_assert(MatrixType::row==VectorType::size, "matrix size != vector size");

    const size_t size = VectorType::size;

    // コレスキー分解
    decompose(mat);

    // 前進代入（forward substitution）
    for (size_t i=0; i<size; ++i)
    {
        for (size_t k=0; k<i; ++k)
        {
            vec[i] -= vec[k] * mat[i][k]; 
        }
        vec[i] /= mat[i][i];
    }

    // 後退代入 (backward substitution）
    for (size_t i=size-1; i<size; --i)
    {
       for (size_t k=i+1; k<size; ++k)
       {
            vec[i] -= vec[k] * mat[k][i]; 
       }
       vec[i] /= mat[i][i];
    }
}

} // end of namespace Math.
} // end of namespace Spidar.

#endif // SPIDAR_CHOLESKY_HPP

// end of file.