wls.h

Go to the documentation of this file.

// -*- c++ -*-

// Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
//
// This file is part of the TooN Library.  This library is free
// software; you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the
// Free Software Foundation; either version 2, or (at your option)
// any later version.

// This library 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 General Public License for more details.

// You should have received a copy of the GNU General Public License along
// with this library; see the file COPYING.  If not, write to the Free
// Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
// USA.

// As a special exception, you may use this file as part of a free software
// library without restriction.  Specifically, if other files instantiate
// templates or use macros or inline functions from this file, or you compile
// this file and link it with other files to produce an executable, this
// file does not by itself cause the resulting executable to be covered by
// the GNU General Public License.  This exception does not however
// invalidate any other reasons why the executable file might be covered by
// the GNU General Public License.

#ifndef TOON_INCLUDE_WLS_H
#define TOON_INCLUDE_WLS_H

#include "TooN.h"
#include "Cholesky.h"
#include "helpers.h"

#include <cmath>

namespace TooN {

    template <int Size=Dynamic, class Precision=DefaultPrecision,
               template<int DecompSize, class DecompPrecision> class Decomposition = Cholesky>
class WLS {
public:

    WLS(int size=0) :
        my_C_inv(size,size),
        my_vector(size),
        my_decomposition(size),
        my_mu(size)
    {
        clear();
    }

    void clear(){
        my_C_inv = Zeros;
        my_vector = Zeros;
    }

    void add_prior(Precision val){
        for(int i=0; i<my_C_inv.num_rows(); i++){
            my_C_inv(i,i)+=val;
        }
    }
  
    template<class B2>
    void add_prior(const Vector<Size,Precision,B2>& v){
        SizeMismatch<Size,Size>::test(my_C_inv.num_rows(), v.size());
        for(int i=0; i<my_C_inv.num_rows(); i++){
            my_C_inv(i,i)+=v[i];
        }
    }

    template<class B2>
    void add_prior(const Matrix<Size,Size,Precision,B2>& m){
        my_C_inv+=m;
    }

    template<class B2>
    inline void add_mJ(Precision m, const Vector<Size, Precision, B2>& J, Precision weight = 1) {
        
        //Upper right triangle only, for speed
        for(int r=0; r < my_C_inv.num_rows(); r++)
        {
            double Jw = weight * J[r];
            my_vector[r] += m * Jw;
            for(int c=r; c < my_C_inv.num_rows(); c++)
                my_C_inv[r][c] += Jw * J[c];
        }
    }

    template<int N, class B1, class B2, class B3>
    inline void add_mJ(const Vector<N,Precision,B1>& m,
                       const Matrix<Size,N,Precision,B2>& J,
                       const Matrix<N,N,Precision,B3>& invcov){
        const Matrix<Size,N,Precision> temp =  J * invcov;
        my_C_inv += temp * J.T();
        my_vector += temp * m;
    }

    template<int N, class B1, class B2, class B3>
    inline void add_mJ_rows(const Vector<N,Precision,B1>& m,
                       const Matrix<N,Size,Precision,B2>& J,
                       const Matrix<N,N,Precision,B3>& invcov){
        const Matrix<Size,N,Precision> temp =  J.T() * invcov;
        my_C_inv += temp * J;
        my_vector += temp * m;
    }

    template<int N, int S1, class B1, class B2, class B3>
    inline void add_sparse_mJ_rows(const Vector<N,Precision,B1>& m,
                       const Matrix<N,S1,Precision,B2>& J1, const int index1,
                       const Matrix<N,N,Precision,B3>& invcov){
        const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
        const int size1 = J1.num_cols();
        my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
        my_vector.slice(index1, size1) += temp1 * m;
    }

    template<int N, int S1, int S2, class B1, class B2, class B3, class B4>
    inline void add_sparse_mJ_rows(const Vector<N,Precision,B1>& m,
                       const Matrix<N,S1,Precision,B2>& J1, const int index1,
                       const Matrix<N,S2,Precision,B3>& J2, const int index2,
                       const Matrix<N,N,Precision,B4>& invcov){
        const Matrix<S1,N,Precision> temp1 = J1.T() * invcov;
        const Matrix<S2,N,Precision> temp2 = J2.T() * invcov;
        const Matrix<S1,S2,Precision> mixed = temp1 * J2;
        const int size1 = J1.num_cols();
        const int size2 = J2.num_cols();
        my_C_inv.slice(index1, index1, size1, size1) += temp1 * J1;
        my_C_inv.slice(index2, index2, size2, size2) += temp2 * J2;
        my_C_inv.slice(index1, index2, size1, size2) += mixed;
        my_C_inv.slice(index2, index1, size2, size1) += mixed.T();
        my_vector.slice(index1, size1) += temp1 * m;
        my_vector.slice(index2, size2) += temp2 * m;
    }

    void compute(){
    
        //Copy the upper right triangle to the empty lower-left.
        for(int r=1; r < my_C_inv.num_rows(); r++)
            for(int c=0; c < r; c++)
                my_C_inv[r][c] = my_C_inv[c][r];

        my_decomposition.compute(my_C_inv);
        my_mu=my_decomposition.backsub(my_vector);
    }

    void operator += (const WLS& meas){
        my_vector+=meas.my_vector;
        my_C_inv += meas.my_C_inv;
    }

    Matrix<Size,Size,Precision>& get_C_inv() {return my_C_inv;}
    const Matrix<Size,Size,Precision>& get_C_inv() const {return my_C_inv;}
    Vector<Size,Precision>& get_mu(){return my_mu;}  
    const Vector<Size,Precision>& get_mu() const {return my_mu;} 
    Vector<Size,Precision>& get_vector(){return my_vector;} 
    const Vector<Size,Precision>& get_vector() const {return my_vector;} 
    Decomposition<Size,Precision>& get_decomposition(){return my_decomposition;} 
    const Decomposition<Size,Precision>& get_decomposition() const {return my_decomposition;} 


private:
    Matrix<Size,Size,Precision> my_C_inv;
    Vector<Size,Precision> my_vector;
    Decomposition<Size,Precision> my_decomposition;
    Vector<Size,Precision> my_mu;

    // comment out to allow bitwise copying
    WLS( WLS& copyof );
    int operator = ( WLS& copyof );
};

}

#endif