matrix_tools.h

mean-shift. pointer sample

/*! \file
  \verbatim
  
    Copyright (c) 2007, Sylvain Paris and Fr閐o Durand

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use, copy,
    modify, merge, publish, distribute, sublicense, and/or sell copies
    of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.

  \endverbatim
*/


#ifndef __MATRIX_TOOLS__
#define __MATRIX_TOOLS__


#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_permutation.h>

#include "geom.h"
#include "math_tools.h"
#include "msg_stream.h"


namespace Matrix_tools{

  using namespace Geometry;
  
  template<typename Real>
  inline Real matrix_determinant(const Real m00,
				 const Real m01,
				 const Real m10,
				 const Real m11);

  
  template<typename Real>
  inline Real matrix_determinant(const Square_matrix<2,Real>& matrix);
  
  template<int N,typename Real>
  void inverse_matrix(const Square_matrix<N,Real>& matrix,
		      Square_matrix<N,Real>* const result);

  
  template<typename Real>
  void inverse_matrix(const Square_matrix<3,Real>& matrix,
		      Square_matrix<3,Real>* const result);

  template<int N,typename Real>
  void inverse_matrix_GSL(const Square_matrix<N,Real>& matrix,
			  Square_matrix<N,Real>* const result);

  
  template<int N,int P,typename Real>
  void pseudo_inverse_matrix(const Matrix<N,P,Real>& matrix,
			     Matrix<P,N,Real>* const result);

  template<int N,int P,typename Real>
  void pseudo_inverse_matrix_GSL(const Matrix<N,P,Real>& matrix,
				 Matrix<P,N,Real>* const result);
  
  template<int N,typename Real>
  void eigen_system(const Square_matrix<N,Real>&     matrix,
		    std::vector<Vec<N,Real> >* const eigen_vector,
		    std::vector<Real>* const         eigen_value);


  //! Compute the planar homography between two sets of 4 homogeneous 2D points.
  template<typename Real>
  void simple_planar_homography(const std::vector<Vec<3,Real> >& from,
				const std::vector<Vec<3,Real> >& to,
				Square_matrix<3,Real>* const     homography);


  template<typename Iterator,int N,typename Real>
  void PCA(Iterator                         begin,
	   Iterator                         end,
	   Vec<N,Real>* const               mean,
	   std::vector<Vec<N,Real> >* const eigen_vector,
	   std::vector<Real>* const         eigen_value,
	   const unsigned int               sampling_rate = 1);

  
  template<int M,int N,typename Real>
  void SVD(const Matrix<M,N,Real>&      matrix,
	   Matrix<M,N,Real>* const      U,
	   Vec<N,Real>* const           S,
	   Square_matrix<N,Real>* const V);

/*
  
  #############################################
  #############################################
  #############################################
  ######                                 ######
  ######   I M P L E M E N T A T I O N   ######
  ######                                 ######
  #############################################
  #############################################
  #############################################
  
*/



  
  
  template<typename Real>
  inline Real matrix_determinant(const Real m00,
				 const Real m01,
				 const Real m10,
				 const Real m11){
    
    return m00 * m11 - m01 * m10;
  }

  
  template<typename Real>
  inline Real matrix_determinant(const Square_matrix<2,Real>& matrix){

    return matrix_determinant(matrix(0,0),matrix(0,1),matrix(1,0),matrix(1,1));
  }


    
  template<typename Real>
  void inverse_matrix(const Square_matrix<3,Real>& matrix,
		      Square_matrix<3,Real>* const result){

    Square_matrix<3,Real>& m = *result;

#define DET(I,J,K,L) matrix_determinant(matrix(I,K),matrix(I,L),matrix(J,K),matrix(J,L))
    
    m(0,0) =  DET(1,2,1,2);
    m(1,0) = -DET(1,2,0,2);
    m(2,0) =  DET(1,2,0,1);

    m(0,1) = -DET(0,2,1,2);
    m(1,1) =  DET(0,2,0,2);
    m(2,1) = -DET(0,2,0,1);

    m(0,2) =  DET(0,1,1,2);
    m(1,2) = -DET(0,1,0,2);
    m(2,2) =  DET(0,1,0,1);

#undef DET

    const Real det = matrix(0,0) * m(0,0) + matrix(0,1) * m(1,0) + matrix(0,2) * m(2,0);

    m /= det;
  }


  
  
  template<int N,typename Real>
  void inverse_matrix(const Square_matrix<N,Real>& matrix,
		      Square_matrix<N,Real>* const result){ 
    typedef Real         real_type;
    typedef unsigned int size_type;
    
    Geometry::Matrix<N,2*N,Real> proxy = Geometry::Matrix<N,2*N,Real>();
    Geometry::Vec<2*N,Real>   vec;

    for(size_type i=0;i<N;i++){
      for(size_type j=0;j<N;j++){
	
	proxy(i,j)   = matrix(i,j);
	proxy(i,j+N) = (i==j) ? 1 : 0;
      }
    }
   
    for(size_type row=0;row<N;row++){

      if (proxy(row,row) == 0){
	
	bool cont = true;
	for(size_type r=0;(r<N)&&cont;r++){
	  
	  if (r==row){
	    r++;
	  }

	  cont = (proxy(r,row) == 0);
	  if(!cont){
	    proxy.swap_rows(r,row);
	  }
	}
      }
      proxy.multiply_row(row,1.0/proxy(row,row));
      vec = proxy.get_vector_from_row(row);

      for(size_type r=0;r<N;r++){
	
	if (r==row){
	  r++;
	}
	if (r<N) proxy.add_vector_to_row(r,-vec*proxy(r,row));
     }

    }  

    for(size_type i=0;i<N;i++){
      for(size_type j=0;j<N;j++){
	(*result)(i,j) = proxy(i,j+N);
      }
    }
  }



  
  template<int N,typename Real>
  void inverse_matrix_GSL(const Square_matrix<N,Real>& matrix,
			  Square_matrix<N,Real>* const result){

    typedef unsigned int size_type;

    gsl_permutation* permutation = gsl_permutation_alloc(N);

    double* gsl_proxy = new double[N*N];
    for(size_type i=0;i<N;i++){
      for(size_type j=0;j<N;j++){
	gsl_proxy[i*N+j] = matrix(i,j);
      }
    }

    gsl_matrix_view input = gsl_matrix_view_array(gsl_proxy,N,N);
    gsl_matrix* output    = gsl_matrix_alloc(N,N);

    gsl_linalg_LU_invert(&input.matrix,permutation,output);
    
    for(size_type i=0;i<N;i++){      
      for(size_type j=0;j<N;j++){
	(*result)(i,j) = gsl_matrix_get(output,i,j);
      }
    }
   
    gsl_matrix_free(output);
    gsl_permutation_free(permutation);
    delete[] gsl_proxy;
  }



  
  template<int N,int P,typename Real>
  void pseudo_inverse_matrix(const Matrix<N,P,Real>& matrix,
			     Matrix<P,N,Real>* const result){

    Matrix<P,N,Real> transpose = matrix.transpose();
    Square_matrix<P,Real> square  = transpose * matrix;
    Square_matrix<P,Real> inverse;
    inverse_matrix(square,&inverse);
    *result = inverse * transpose;
  }

  

  template<int N,int P,typename Real>
  void pseudo_inverse_matrix_GSL(const Matrix<N,P,Real>& matrix,
				 Matrix<P,N,Real>* const result){
    using namespace std;
    Matrix<P,N,Real> transpose = matrix.transpose();

    Square_matrix<P,Real> square  = transpose * matrix;
   
    Square_matrix<P,Real> inverse;
    inverse_matrix_GSL(square,&inverse);
   
    *result = inverse * transpose;
  }

  

  template<int N,typename Real>
  void eigen_system(const Square_matrix<N,Real>&     matrix,
		    std::vector<Vec<N,Real> >* const eigen_vector,
		    std::vector<Real>* const         eigen_value){

    typedef unsigned int size_type;
    typedef Real         real_type;

    eigen_vector->resize(N);
    eigen_value->resize(N);
    
    double* gsl_proxy = new double[N*N];

    for(size_type i=0;i<N;i++){
      for(size_type j=0;j<N;j++){
	gsl_proxy[i*N+j] = matrix(i,j);
      }
    }

    gsl_matrix_view gsl_mat      = gsl_matrix_view_array(gsl_proxy,N,N);
    gsl_vector* gsl_eigen_value  = gsl_vector_alloc(N);
    gsl_matrix* gsl_eigen_vector = gsl_matrix_alloc(N,N);
    gsl_eigen_symmv_workspace* w = gsl_eigen_symmv_alloc(N);

    gsl_eigen_symmv(&gsl_mat.matrix,gsl_eigen_value,gsl_eigen_vector,w);
    gsl_eigen_symmv_sort(gsl_eigen_value,gsl_eigen_vector,GSL_EIGEN_SORT_ABS_ASC);

    for(size_type i=0;i<N;i++){

      (*eigen_value)[i] = gsl_vector_get(gsl_eigen_value,i);
      
      for(size_type j=0;j<N;j++){
	(*eigen_vector)[i][j] = gsl_matrix_get(gsl_eigen_vector,j,i);
      }
    }
    
    gsl_matrix_free(gsl_eigen_vector);
    gsl_vector_free(gsl_eigen_value);
    gsl_eigen_symmv_free(w);   
    delete[] gsl_proxy;
  }


  //! Adapted from http://www.robots.ox.ac.uk/~vgg/presentations/bmvc97/criminispaper/node3.html
  template<typename Real>
  void simple_planar_homography(const std::vector<Vec3<Real> >& from,
				const std::vector<Vec3<Real> >& to,
				Square_matrix<3,Real>* const    homography){

    typedef Real         real_type;
    typedef unsigned int size_type;
    
    if ((from.size()!=4)||(to.size()!=4)){
      Message::error<<"simple_planar_homography: 'from' and/or 'to' vector lists do not contain 4 elements."<<Message::done;
    }

    Geometry::Matrix<8,9,real_type> linear_sys;

//     for(size_type i=0;i<4;i++){
//       linear_sys(2*i,0) = from[i].x() / from[i].z();
//       linear_sys(2*i,1) = from[i].y() / from[i].z();
//       linear_sys(2*i,2) = 1;
//       linear_sys(2*i,6) = -from[i].x() * to[i].x();
//       linear_sys(2*i,7) = -from[i].y() * to[i].x();
//       linear_sys(2*i,8) = -to[i].x();
	
//       linear_sys(2*i+1,3) = from[i].x() / from[i].z();
//       linear_sys(2*i+1,4) = from[i].y() / from[i].z();
//       linear_sys(2*i+1,5) = 1;
//       linear_sys(2*i+1,6) = -from[i].x() * to[i].y();
//       linear_sys(2*i+1,7) = -from[i].y() * to[i].y();
//       linear_sys(2*i+1,8) = -to[i].y();    
//     }
    for(size_type i=0;i<4;i++){
      linear_sys(2*i,0) = from[i].x() * to[i].z();
      linear_sys(2*i,1) = from[i].y() * to[i].z();
      linear_sys(2*i,2) = from[i].z() * to[i].z();
      linear_sys(2*i,6) = -from[i].x() * to[i].x();
      linear_sys(2*i,7) = -from[i].y() * to[i].x();
      linear_sys(2*i,8) = -from[i].z() * to[i].x();
	
      linear_sys(2*i+1,3) = from[i].x() * to[i].z();
      linear_sys(2*i+1,4) = from[i].y() * to[i].z();
      linear_sys(2*i+1,5) = from[i].z() * to[i].z();
      linear_sys(2*i+1,6) = -from[i].x() * to[i].y();
      linear_sys(2*i+1,7) = -from[i].y() * to[i].y();
      linear_sys(2*i+1,8) = -from[i].z() * to[i].y();    
    }

    std::vector<Geometry::Vec<9,real_type> > eigen_vector(9);
    std::vector<real_type>                   eigen_value(9);
      
    Matrix_tools::eigen_system(Geometry::Square_matrix<9,real_type>(linear_sys.transpose()*linear_sys),
			       &eigen_vector,&eigen_value);

    for(size_type i=0;i<3;i++){
      for(size_type j=0;j<3;j++){
	(*homography)(i,j) = eigen_vector[0][3*i+j];
      }
    }

  }



  template<typename Iterator,int N,typename Real>
  void PCA(Iterator                         begin,
	   Iterator                         end,
	   Vec<N,Real>* const               mean,
	   std::vector<Vec<N,Real> >* const eigen_vector,
	   std::vector<Real>* const         eigen_value,
	   const unsigned int               sampling_rate){

    typedef Real         real_type;
    typedef unsigned int size_type;
    typedef Vec<N,Real> vector_type;
    typedef std::vector<vector_type> vector_list_type;
    
    const size_type size = 1 + distance(begin,end) / sampling_rate;
    vector_list_type p(size);
    Iterator p_i = begin;
    for(size_type i = 0;i < size;++i,p_i += sampling_rate){
      p[i] = *p_i;
    }
        
    *mean = Math_tools::mean(p.begin(),p.end());

    for(typename vector_list_type::iterator
	  i     = p.begin(),
	  i_end = p.end();
	i != i_end;
	++i){

      *i -= *mean;
    }

    Square_matrix<N,Real> covariance;
  
    for(size_type n = 0,n_end = p.size();n < n_end;++n){

      for(size_type i = 0;i < N;++i){

	const real_type p_i = p[n][i];
      
	covariance(i,i) += p_i * p_i;
      
	for(size_type j=i+1;j<N;j++){

	  const real_type cov_ij = p_i * p[n][j];
	
	  covariance(i,j) += cov_ij;
	  covariance(j,i) += cov_ij;
	} // END OF for j
      } // END OF for i
    } // END OF for n

    eigen_system(covariance,eigen_vector,eigen_value);
  
  } // END OF function PCA



  template<int M,int N,typename Real>
  void SVD(const Matrix<M,N,Real>&      matrix,
	   Matrix<M,N,Real>* const      U,
	   Vec<N,Real>* const           S,
	   Square_matrix<N,Real>* const V){

    typedef Real         real_type;
    typedef unsigned int size_type;

    gsl_matrix* gsl_A = gsl_matrix_alloc(M,N);
    gsl_matrix* gsl_V = gsl_matrix_alloc(N,N);
    gsl_vector* gsl_S = gsl_vector_alloc(N);
    gsl_vector* gsl_W = gsl_vector_alloc(N);

    for(size_type i = 0;i < M;++i){
      for(size_type j = 0;j < N;++j){
	gsl_matrix_set(gsl_A,i,j,matrix(i,j));
      }
    }

    gsl_linalg_SV_decomp(gsl_A,gsl_V,gsl_S,gsl_W);
    
    for(size_type i = 0;i < M;++i){
      for(size_type j = 0;j < N;++j){
	(*U)(i,j) = gsl_matrix_get(gsl_A,i,j);
      }
    }
    
    for(size_type i = 0;i < N;++i){

      (*S)[i] = gsl_vector_get(gsl_S,i);
      
      for(size_type j = 0;j < N;++j){
	(*V)(i,j) = gsl_matrix_get(gsl_V,i,j);
      }
    }
    
    gsl_matrix_free(gsl_A);
    gsl_matrix_free(gsl_V);
    gsl_vector_free(gsl_S);
    gsl_vector_free(gsl_W);
  }


  

} // END OF namespace Matrix_tools



#endif