Cholesky.h

// -*- c++ -*-

//     Copyright (C) 2009 Tom Drummond (twd20@cam.ac.uk)

//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:
//1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//2. Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND OTHER CONTRIBUTORS ``AS IS''
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR OTHER CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.


#ifndef TOON_INCLUDE_CHOLESKY_H
#define TOON_INCLUDE_CHOLESKY_H

#include <TooN/TooN.h>

namespace TooN {


template <int Size=Dynamic, class Precision=DefaultPrecision>
class Cholesky {
public:
    Cholesky(){}

    template<class P2, class B2>
    Cholesky(const Matrix<Size, Size, P2, B2>& m)
        : my_cholesky(m) {
        SizeMismatch<Size,Size>::test(m.num_rows(), m.num_cols());
        do_compute();
    }
    
    Cholesky(int size) : my_cholesky(size,size) {}


    template<class P2, class B2> void compute(const Matrix<Size, Size, P2, B2>& m){
        SizeMismatch<Size,Size>::test(m.num_rows(), m.num_cols());
        SizeMismatch<Size,Size>::test(m.num_rows(), my_cholesky.num_rows());
        my_cholesky=m;
        do_compute();
    }
    
    private:
    void do_compute() {
        int size=my_cholesky.num_rows();
        for(int col=0; col<size; col++){
            Precision inv_diag = 1;
            for(int row=col; row < size; row++){
                // correct for the parts of cholesky already computed
                Precision val = my_cholesky(row,col);
                for(int col2=0; col2<col; col2++){
                    // val-=my_cholesky(col,col2)*my_cholesky(row,col2)*my_cholesky(col2,col2);
                    val-=my_cholesky(col2,col)*my_cholesky(row,col2);
                }
                if(row==col){
                    // this is the diagonal element so don't divide
                    my_cholesky(row,col)=val;
                    if(val == 0){
                        my_rank = row;
                        return;
                    }
                    inv_diag=1/val;
                } else {
                    // cache the value without division in the upper half
                    my_cholesky(col,row)=val;
                    // divide my the diagonal element for all others
                    my_cholesky(row,col)=val*inv_diag;
                }
            }
        }
        my_rank = size;
    }

    public:

    template<int Size2, class P2, class B2>
    Vector<Size, Precision> backsub (const Vector<Size2, P2, B2>& v) const {
        int size=my_cholesky.num_rows();
        SizeMismatch<Size,Size2>::test(size, v.size());

        // first backsub through L
        Vector<Size, Precision> y(size);
        for(int i=0; i<size; i++){
            Precision val = v[i];
            for(int j=0; j<i; j++){
                val -= my_cholesky(i,j)*y[j];
            }
            y[i]=val;
        }
        
        // backsub through diagonal
        for(int i=0; i<size; i++){
            y[i]/=my_cholesky(i,i);
        }

        // backsub through L.T()
        Vector<Size,Precision> result(size);
        for(int i=size-1; i>=0; i--){
            Precision val = y[i];
            for(int j=i+1; j<size; j++){
                val -= my_cholesky(j,i)*result[j];
            }
            result[i]=val;
        }

        return result;
    }

    template<int Size2, int C2, class P2, class B2>
    Matrix<Size, C2, Precision> backsub (const Matrix<Size2, C2, P2, B2>& m) const {
        int size=my_cholesky.num_rows();
        SizeMismatch<Size,Size2>::test(size, m.num_rows());

        // first backsub through L
        Matrix<Size, C2, Precision> y(size, m.num_cols());
        for(int i=0; i<size; i++){
            Vector<C2, Precision> val = m[i];
            for(int j=0; j<i; j++){
                val -= my_cholesky(i,j)*y[j];
            }
            y[i]=val;
        }
        
        // backsub through diagonal
        for(int i=0; i<size; i++){
            y[i]*=(1/my_cholesky(i,i));
        }

        // backsub through L.T()
        Matrix<Size,C2,Precision> result(size, m.num_cols());
        for(int i=size-1; i>=0; i--){
            Vector<C2,Precision> val = y[i];
            for(int j=i+1; j<size; j++){
                val -= my_cholesky(j,i)*result[j];
            }
            result[i]=val;
        }
        return result;
    }


    // easy way to get inverse - could be made more efficient
    Matrix<Size,Size,Precision> get_inverse(){
        Matrix<Size,Size,Precision>I(Identity(my_cholesky.num_rows()));
        return backsub(I);
    }
    
    Precision determinant(){
        Precision answer=my_cholesky(0,0);
        for(int i=1; i<my_cholesky.num_rows(); i++){
            answer*=my_cholesky(i,i);
        }
        return answer;
    }

    template <int Size2, typename P2, typename B2>
    Precision mahalanobis(const Vector<Size2, P2, B2>& v) const {
        return v * backsub(v);
    }

    Matrix<Size,Size,Precision> get_unscaled_L() const {
        Matrix<Size,Size,Precision> m(my_cholesky.num_rows(),
                          my_cholesky.num_rows());
        m=Identity;
        for (int i=1;i<my_cholesky.num_rows();i++) {
            for (int j=0;j<i;j++) {
                m(i,j)=my_cholesky(i,j);
            }
        }
        return m;
    }
            
    Matrix<Size,Size,Precision> get_D() const {
        Matrix<Size,Size,Precision> m(my_cholesky.num_rows(),
                          my_cholesky.num_rows());
        m=Zeros;
        for (int i=0;i<my_cholesky.num_rows();i++) {
            m(i,i)=my_cholesky(i,i);
        }
        return m;
    }
    
    Matrix<Size,Size,Precision> get_L() const {
        using std::sqrt;
        Matrix<Size,Size,Precision> m(my_cholesky.num_rows(),
                          my_cholesky.num_rows());
        m=Zeros;
        for (int j=0;j<my_cholesky.num_cols();j++) {
            Precision sqrtd=sqrt(my_cholesky(j,j));
            m(j,j)=sqrtd;
            for (int i=j+1;i<my_cholesky.num_rows();i++) {
                m(i,j)=my_cholesky(i,j)*sqrtd;
            }
        }
        return m;
    }

    int rank() const { return my_rank; }

private:
    Matrix<Size,Size,Precision> my_cholesky;
    int my_rank;
};


}

#endif