PCAT_impl.hh

/*===========================================================================*\
*                                                                            *
*                              OpenFlipper                                   *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openflipper.org                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenFlipper.                                         *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * 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.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 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 COPYRIGHT HOLDER *
 * OR 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.              *
*                                                                            *
\*===========================================================================*/






//=============================================================================
//
//  CLASS PCA - IMPLEMENTATION
//
//=============================================================================

#define PCA_C

//== INCLUDES =================================================================

#include "PCA.hh"

extern "C" {

    typedef void (*U_fp)(...);

    int dsyev_( char *jobz, char *uplo, long int *n, double *a,
                long int *lda, double *w, double *work, long int *lwork,
                long int *info);

}

// typedef gmm::dense_matrix< double > Matrix;
// typedef std::vector< double > Vector;

static double * p_work_double = 0;

static int m_workSize_double = 0;

//== NAMESPACES ===============================================================

namespace Pca {

//== IMPLEMENTATION ==========================================================

template < typename VectorT >
PCA< VectorT >::PCA( std::vector< VectorT >& _points , VectorT& _first , VectorT& _second , VectorT& _third)
{
  pca(_points,_first,_second,_third);
}

template < typename VectorT >
PCA< VectorT >::~PCA()
{
  if ( p_work_double != 0){
    delete [] p_work_double;
    p_work_double = 0;
    m_workSize_double = 0;
  }
}

template < typename VectorT >
inline
VectorT  
PCA< VectorT >::
center_of_gravity(const std::vector< VectorT >& _points ) {
   // compute cog of Points
   VectorT cog(0.0, 0.0, 0.0);
   
   for (uint i = 0 ; i < _points.size() ; ++i ) {
     cog = cog + _points[i];
   }
   
   cog = cog / ( typename VectorT::value_type )_points.size();

   return cog;
}

template < typename VectorT >
bool 
PCA< VectorT >::SymRightEigenproblem( Matrix &_mat_A, Matrix & _mat_VR,
                           std::vector< double > & _vec_EV )
{
    char jobz, uplo;
    long int n, lda;
    long int lwork, info;
    double size;

    jobz = 'V';
    uplo = 'U';
    n = gmm::mat_nrows( _mat_A );
    lda = n;

    info = 0;
    // compute optimal size of working array
    lwork = -1;
    dsyev_( &jobz, &uplo, &n, &_mat_A( 0, 0 ), &lda, &_vec_EV[ 0 ],
            &size, &lwork, &info );

    if( info != 0 )
        return false;


    if( (long int) size > m_workSize_double )
    {
        if( p_work_double)
            delete [] p_work_double;

        m_workSize_double = (long int) size;

        p_work_double= new double[ m_workSize_double ];
    }
    lwork = m_workSize_double;

    // compute eigenvalues / eigenvectors
    dsyev_( &jobz, &uplo, &n, &_mat_A( 0, 0 ), &lda, &_vec_EV[ 0 ],
            p_work_double, &lwork, &info );

    if( info != 0 )
        return false;

    // copy eigenvectors to matrix
    gmm::copy( _mat_A, _mat_VR );

    return true;
}


template < typename VectorT >
void 
PCA< VectorT >::pca(std::vector< VectorT >& _points , VectorT& _first , VectorT& _second , VectorT& _third)
{
   // compute Mean of Points
   const VectorT cog = center_of_gravity(_points);
   
   //build covariance Matrix
   Matrix points(3 , _points.size() );
  
   for ( uint i = 0 ; i < _points.size() ; ++i)
   {
       points(0 , i ) = ( _points[i] - cog) [0];
       points(1 , i ) = ( _points[i] - cog) [1];
       points(2 , i ) = ( _points[i] - cog) [2];
   }
  
  Matrix cov(3,3);
  gmm::mult(points,gmm::transposed(points),cov );
  
  Matrix EV(3,3);
  std::vector< double > ew(3);
  

  if ( !SymRightEigenproblem( cov, EV ,ew ) )
    std::cerr << "Error: Could not compute Eigenvectors for PCA" << std::endl;

  int maximum,middle,minimum;
  
  if ( (ew[0] > ew[1]) && (ew[0] > ew[2]) ) {
    maximum = 0;
  } else {
    if ( (ew[1] > ew[0]) && (ew[1] > ew[2]) )
         maximum = 1;
       else
         maximum = 2;
  }
  
  if ( (ew[0] < ew[1]) && (ew[0] < ew[2]) )
    minimum = 0;
  else if ( (ew[1] < ew[0]) && (ew[1] < ew[2]) )
         minimum = 1;
       else
         minimum = 2;         

  if ( (minimum != 0 ) && ( maximum != 0 ) )       
    middle = 0;
  else  
    if ( (minimum != 1 ) && ( maximum != 1 ) )       
      middle = 1;
    else
      middle = 2;
  
  _first  = VectorT( EV(0,maximum) , EV(1,maximum) , EV(2,maximum) );
  _second = VectorT( EV(0,middle)  , EV(1,middle)  , EV(2,middle ) );

  _first  = _first.normalize();
  _second = _second.normalize();
  _third  = _first % _second; 

  //remember the eigenvalues
  lastEigenValues_.clear();
  lastEigenValues_.push_back( ew[maximum] );
  lastEigenValues_.push_back( ew[middle] );
  lastEigenValues_.push_back( ew[minimum] );
}
//-----------------------------------------------------------------------------

template < typename VectorT >
inline
std::vector<double>&
PCA< VectorT >::getLastEigenValues()
{
  return lastEigenValues_;
}


//=============================================================================

} //namespace Pca

//=============================================================================