ublasTools.h

Go to the documentation of this file.

/******************************************************************************
 *
 *  Copyright (C) 2008 Max Hermann
 *
 *  This file is part of the "CEDRIC Event Display" application.
 *
 *****************************************************************************/

#ifndef UBLASTOOLS_H
#define UBLASTOOLS_H

// includes for affine_fit
#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/operation.hpp>
#include <boost/numeric/ublas/lu.hpp> 

namespace ublas = boost::numeric::ublas;

#include <iostream>
#include <assert.h>


namespace ublasTools
{
template<class ValueType>
bool affine_fit( const ublas::matrix<ValueType>& q_, const ublas::matrix<ValueType>& p, 
                 ublas::matrix<ValueType>& A, ublas::vector<ValueType>& b )
{
        assert( (q_.size1()==p.size1()) && (q_.size2()==p.size2()) );

        using namespace ublas;
        
        unsigned int n = q_.size1();
        unsigned int m = q_.size2();
        
        // append row of ones to q
        ublas::matrix<ValueType> q = q_;
        q.resize( n+1, m );
        for( unsigned int i=0; i < m; ++i )
                q(n,i) = 1.0;

        // build product matrix $Q = \sum_{j=1}^m q_j q_j^T$
        // $$Q_{ji} = Q_{ij} = \sum_{k=1}^m q_{ik}q{jk}$$
        ublas::matrix<ValueType> Q( n+1, n+1 );
        for( unsigned int i=0; i < n+1; ++i )
                for( unsigned int j=0; j <= i; ++j )
                {
                        Q(i,j) = 0.0;
                        for( unsigned int k=0; k < m; ++k )
                                Q(i,j) += q(i,k)*q(j,k);
                        
                        // symmetric
                        Q(j,i) = Q(i,j);
                }

        // build matrix $C = (C_1,C_2,\ldots,C_n)$ where $C_j = (C_{1j},C_{2j},\ldots,C_{n+1,j})^T$
        // $$C_{kj} = \sum_{i=1}^{m}q_{ki}p_{ji}$$
        ublas::matrix<ValueType> C( n+1, n );
        for( unsigned int k=0; k < n+1; ++k )
                for( unsigned int j=0; j < n; ++j )
                {
                        C(k,j) = 0.0;
                        for( unsigned int i=0; i < m; ++i )
                                C(k,j) += q(k,i)*p(j,i);
                }
        
        // compute inverse of $Q$ via LU decomposition

        // LU decompose $Q=LU$ 
        // We use boost's lu_factorize with partial pivoting, but this is needless for spd matrices,
        // and even destroys the nice spd structure which Q usually has.
        permutation_matrix<double> perm(n+1);
        if( lu_factorize( Q, perm ) != 0 )
        {
                std::cout << "Stumbled upon a singular matrix during LU decomposition!" << std::endl;  //<=========== TODO: throw exception ==============>
                return false;
        };
        
        // backsubstitute to get the inverse
        ublas::matrix<ValueType> inverse;
        inverse = identity_matrix<double>(Q.size1());
        lu_substitute( Q, perm, inverse );
        
        // compute $\tilde{A}=Q^{-1}C$
        A.resize( n+1, n );
        A = prod( inverse, C );
        
        // extract $A$ and $b$ from $\tilde{A}$
        b = row( A, n );   // last row of $\tilde{A}$ hold translation $b$
        A.resize( n, n );  // remove last row
        A = trans( A );    // $\tilde{A}$ contains $A^t$, so we have to transpose to get $A$
        
        return true;
}
        
template<class ValueType>
ublas::vector<ValueType> column_means( ublas::matrix<ValueType>& A )
{
        using namespace ublas;
                
        unsigned int m = A.size1(); // number of rows
        unsigned int n = A.size2(); // number of columns
        
        // compute column means
        
        ublas::vector<ValueType> mean(n);
        mean = zero_vector<ValueType>(n);
        
        for( unsigned int i=0; i < m; ++i )
                mean += matrix_row<matrix<ValueType> >( A, i );
        
        mean *= 1.0 / m;
        
        return mean;
}

template<class ValueType>
ublas::vector<ValueType> row_means( ublas::matrix<ValueType>& A )
{
        using namespace ublas;
                
        unsigned int m = A.size1(); // number of rows
        unsigned int n = A.size2(); // number of columns
        
        // compute row means
        
        ublas::vector<ValueType> mean(m);
        mean = zero_vector<ValueType>(m);
        
        for( unsigned int j=0; j < n; ++j )
                mean += matrix_column<matrix<ValueType> >( A, j );
        
        mean *= 1.0 / n;
        
        return mean;
}

template<class ValueType>
ValueType grand_mean( const ublas::matrix<ValueType>& A )
{
        double gm = 0.0;
        for( unsigned int i=0; i < A.size1(); ++i )
                for( unsigned int j=0; j < A.size2(); ++j )
                        gm += A(i,j);
        
        gm *= 1.0 / (A.size1()*A.size2());
                
        return gm;
}

template<class ValueType>
void double_center( ublas::matrix<ValueType>& D )
{
        typedef ublas::vector<ValueType> Vector;
        
        // compute means of component squared D
        Vector cmeans = column_means( D );
        Vector rmeans = row_means( D ); 
        double gmean  = grand_mean( D );

        //~ cout << "column means = " << cmeans << endl;
        //~ cout << "row means    = " << rmeans << endl;
        //~ cout << "grand mean   = " << gmean  << endl;        
                
        // double center matrix D
        for( unsigned int i=0; i < D.size1(); ++i )
                for( unsigned int j=0; j < D.size2(); ++j )
                {
                        D(i,j) = D(i,j) - cmeans(i) - rmeans(j) + gmean;
                }       
}

template<class ValueType>
void zero_eps( ublas::matrix<ValueType>& A, ValueType eps = 1e-7 )
{
        for( unsigned int i=0; i < A.size1(); ++i )
                for( unsigned int j=0; j < A.size2(); ++j )
                {
                        if( fabs(A(i,j)) < eps ) A(i,j)=0.0;
                }
}

template<class ValueType>
void zero_eps( ublas::vector<ValueType>& v, ValueType eps = 1e-7 )
{
        for( unsigned int i=0; i < v.size(); ++i )
                if( fabs(v(i)) < eps ) v(i)=0.0;
}

} // namespace ublasTools

#endif // UBLASTOOLS_H