Reinvent the wheel.

#pragma once
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <math.h>

#define PI 3.14159265358979323846

template<class matrix_T>
double determinant(const boost::numeric::ublas::matrix_expression<matrix_T> & mat_r)
{
    double det = 1.0;

    matrix_T mLu(mat_r() );
    boost::numeric::ublas::permutation_matrix<std::size_t> pivots(mat_r().size1() );

    int is_singular = lu_factorize(mLu, pivots);

    if (!is_singular)
    {
	for (std::size_t i=0; i < pivots.size(); ++i)
	{
	    if (pivots(i) != i)
		det *= -1.0;

	    det *= mLu(i,i);
	}
    }
    else
	det = 0.0;

    return det;
}

 /* Matrix inversion routine.
    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
 template<class T>
 bool invertMatrix(const boost::numeric::ublas::matrix<T>& input, 
		   boost::numeric::ublas::matrix<T>& inverse) {
 	using namespace boost::numeric::ublas;
 	typedef permutation_matrix<std::size_t> pmatrix;
 	// create a working copy of the input
 	matrix<T> A(input);
 	// create a permutation matrix for the LU-factorization
 	pmatrix pm(A.size1());
 	// perform LU-factorization
 	int res = lu_factorize(A,pm);
        if( res != 0 ) return false;
 	// create identity matrix of "inverse"
 	inverse.assign(identity_matrix<T>(A.size1()));
 	// backsubstitute to get the inverse
 	lu_substitute(A, pm, inverse);
 	return true;
 }

template<class T>
double  gaussian(const boost::numeric::ublas::vector<T> & f,
		 const boost::numeric::ublas::vector<T> & mu,
		 const boost::numeric::ublas::matrix<T> & sigma)
{
    boost::numeric::ublas::matrix<T> sigma_inverse(sigma.size1(), sigma.size2());
    invertMatrix(sigma, sigma_inverse);

    boost::numeric::ublas::vector<T> t = f-mu;

    double e = inner_prod(prod(trans(t), sigma_inverse), t);
    double d = pow(2*PI, f.size()/2.0)*sqrt(determinant(sigma));

    return 1/d*exp(-0.5*e);
}

 

Advertisements