vpMatrix.cpp

/****************************************************************************
*
* $Id: vpMatrix.cpp 5238 2015-01-30 13:52:25Z fspindle $
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
* 
* This software is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* ("GPL") version 2 as published by the Free Software Foundation.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact INRIA about acquiring a ViSP Professional 
* Edition License.
*
* See http://www.irisa.fr/lagadic/visp/visp.html for more information.
* 
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
* http://www.irisa.fr/lagadic
*
* If you have questions regarding the use of this file, please contact
* INRIA at visp@inria.fr
* 
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
*
* Description:
* Matrix manipulation.
*
* Authors:
* Eric Marchand
*
*****************************************************************************/



/*
\class vpMatrix

\brief Provide simple Matrices computation

\author Eric Marchand   (Eric.Marchand@irisa.fr) Irisa / Inria Rennes
*/

#include <visp/vpMatrix.h>
#include <visp/vpMath.h>
#include <visp/vpHomography.h>
#include <visp/vpTranslationVector.h>

// Exception
#include <visp/vpException.h>
#include <visp/vpMatrixException.h>

// Debug trace
#include <visp/vpDebug.h>

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <vector>
#include <sstream>
#include <algorithm>
#include <assert.h>
#include <fstream>
#include <string>
#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

#ifdef VISP_HAVE_GSL
#include <gsl/gsl_linalg.h>
#endif

#define DEBUG_LEVEL1 0
#define DEBUG_LEVEL2 0


//Prototypes of specific functions
vpMatrix subblock(const vpMatrix &, unsigned int, unsigned int);

void
vpMatrix::init()
{

  rowNum = 0  ;
  colNum = 0 ;

  data = NULL ;
  rowPtrs = NULL ;

  dsize = 0 ;
  trsize =0 ;
}

vpMatrix::vpMatrix()
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
}


vpMatrix::vpMatrix(unsigned int r, unsigned int c)
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
  resize(r, c);
}

vpMatrix::vpMatrix(unsigned int r, unsigned int c, double val)
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
  resize(r, c);
  *this = val;
}

vpMatrix::vpMatrix(const vpHomography& H)
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
  (*this) = H;
}

vpMatrix::vpMatrix(const vpMatrix &m,
                   unsigned int r, unsigned int c, 
                   unsigned int nrows, unsigned int ncols)
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
  if (((r + nrows) > m.rowNum) || ((c + ncols) > m.colNum))
  {
    vpERROR_TRACE("\n\t\t SubvpMatrix larger than vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::subMatrixError,
      "\n\t\t SubvpMatrix larger than vpMatrix")) ;
  }

  init(m,r,c,nrows,ncols);
}

vpMatrix::vpMatrix(const vpMatrix& m)
  : rowNum(0), colNum(0), data(NULL), rowPtrs(NULL), dsize(0), trsize(0)
{
  resize(m.rowNum,m.colNum);

  memcpy(data,m.data,rowNum*colNum*sizeof(double)) ;
}


void vpMatrix::resize(const unsigned int nrows, const unsigned int ncols, 
                      const bool flagNullify)
{

  if ((nrows == rowNum) && (ncols == colNum))
  {
    if (flagNullify && this->data != NULL)
    { memset(this->data,0,this->dsize*sizeof(double)) ;}
  }
  else
  {
    const bool recopyNeeded = (ncols != this ->colNum);
    double * copyTmp = NULL;
    unsigned int rowTmp = 0, colTmp=0;

    vpDEBUG_TRACE (25, "Recopy case per case is required iff number of "
      "cols has changed (structure of double array is not "
      "the same in this case.");
    if (recopyNeeded && this->data != NULL)
    {
      copyTmp = new double[this->dsize];
      memcpy (copyTmp, this ->data, sizeof(double)*this->dsize);
      rowTmp=this->rowNum; colTmp=this->colNum;
    }

    vpDEBUG_TRACE (25, "Reallocation of this->data array.");
    this->dsize = nrows*ncols;
    this->data = (double*)realloc(this->data, this->dsize*sizeof(double));
    if ((NULL == this->data) && (0 != this->dsize))
    {
      if (copyTmp != NULL) delete [] copyTmp;
      vpERROR_TRACE("\n\t\tMemory allocation error when allocating data") ;
      throw(vpException(vpException::memoryAllocationError,
        "\n\t\t Memory allocation error when "
        "allocating data")) ;
    }

    vpDEBUG_TRACE (25, "Reallocation of this->trsize array.");
    this->trsize = nrows;
    this->rowPtrs = (double**)realloc (this->rowPtrs, this->trsize*sizeof(double*));
    if ((NULL == this->rowPtrs) && (0 != this->dsize))
    {
      if (copyTmp != NULL) delete [] copyTmp;
      vpERROR_TRACE("\n\t\tMemory allocation error when allocating rowPtrs") ;
      throw(vpException(vpException::memoryAllocationError,
        "\n\t\t Memory allocation error when "
        "allocating rowPtrs")) ;
    }

    vpDEBUG_TRACE (25, "Recomputation this->trsize array values.");
    {
      double **t_= rowPtrs;
      for (unsigned int i=0; i<dsize; i+=ncols)  { *t_++ = this->data + i; }
    }

    this->rowNum = nrows; this->colNum = ncols;

    vpDEBUG_TRACE (25, "Recopy of this->data array values or nullify.");
    if (flagNullify)
    { memset(this->data,0,this->dsize*sizeof(double)) ;}
    else
    {
      if (recopyNeeded && this->rowPtrs != NULL)
      {
        vpDEBUG_TRACE (25, "Recopy...");
        const unsigned int minRow = (this->rowNum<rowTmp)?this->rowNum:rowTmp;
        const unsigned int minCol = (this->colNum<colTmp)?this->colNum:colTmp;
        for (unsigned int i=0; i<this->rowNum; ++i)
          for (unsigned int j=0; j<this->colNum; ++j)
          {
            if ((minRow > i) && (minCol > j))
            {
              (*this)[i][j] = copyTmp [i*colTmp+j];
              vpCDEBUG (25) << i << "x" << j << "<- " << i*colTmp+j
                << "=" << copyTmp [i*colTmp+j] << std::endl;
            }
            else {(*this)[i][j] = 0;}
          }
      }
      else { vpDEBUG_TRACE (25,"Nothing to do: already done by realloc.");}
    }

    if (copyTmp != NULL) delete [] copyTmp;
  }

}

void
vpMatrix::init(const vpMatrix &M,unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
{
  unsigned int rnrows = r+nrows ;
  unsigned int cncols = c+ncols ;

  if (rnrows > M.getRows())
    throw(vpException(vpException::dimensionError,
                      "Bad row dimension (%d > %d) used to initialize vpMatrix", rnrows, M.getRows()));
  if (cncols > M.getCols())
    throw(vpException(vpException::dimensionError,
                      "Bad column dimension (%d > %d) used to initialize vpMatrix", cncols, M.getCols()));
  resize(nrows, ncols);

  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
    return; // Noting to do
  for (unsigned int i=r ; i < rnrows; i++)
    for (unsigned int j=c ; j < cncols; j++)
      (*this)[i-r][j-c] = M[i][j] ;
}

void
vpMatrix::kill()
{
  if (data != NULL )
  {
    free(data);
    data=NULL;
  }

  if (rowPtrs!=NULL)
  {
    free(rowPtrs);
    rowPtrs=NULL ;
  }
}
vpMatrix::~vpMatrix()
{
  kill() ;
}


vpMatrix &
vpMatrix::operator=(const vpMatrix &B)
{
  try {
    resize(B.rowNum, B.colNum) ;
    // suppress by em 5/12/06
    //    *this = 0;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  memcpy(data,B.data,dsize*sizeof(double)) ;

  return *this;
}

vpMatrix &
vpMatrix::operator=(const vpHomography& H)
{
  init() ;
  resize(3,3);
  for(unsigned int i=0; i<3; i++)
    for(unsigned int j=0; j<3; j++)
      (*this)[i][j] = H[i][j];

  return *this;
}

vpMatrix &
vpMatrix::operator=(double x)
{

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //    {
  //        *(data+i) = x;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< std::endl;

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j] = x;

  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";



  return *this;
}


vpMatrix &
vpMatrix::operator<<( double *x )
{

  for (unsigned int i=0; i<rowNum; i++) {
    for (unsigned int j=0; j<colNum; j++) {
      rowPtrs[i][j] = *x++;
    }
  }
  return *this;
}


//---------------------------------
// Matrix operations.
//---------------------------------

void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
  try 
  {
    if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if (A.colNum != B.rowNum)
  {
    vpERROR_TRACE("\n\t\tvpMatrix mismatch in vpMatrix/vpMatrix multiply") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\tvpMatrix mismatch in "
      "vpMatrix/vpMatrix multiply")) ;
  }

  // 5/12/06 some "very" simple optimization to avoid indexation
  unsigned int BcolNum = B.colNum;
  unsigned int BrowNum = B.rowNum;
  unsigned int i,j,k;
  double **BrowPtrs = B.rowPtrs;
  for (i=0;i<A.rowNum;i++)
  {
    double *rowptri = A.rowPtrs[i];
    double *ci = C[i];
    for (j=0;j<BcolNum;j++)
    {
      double s = 0;
      for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
      ci[j] = s;
    }
  }
}

vpMatrix vpMatrix::operator*(const vpMatrix &B) const
{
  vpMatrix C;

  vpMatrix::mult2Matrices(*this,B,C);

  return C;
}
vpMatrix vpMatrix::operator*(const vpHomography &H) const
{
  if (colNum != 3)
    throw(vpException(vpMatrixException::dimensionError, "Cannot multiply the matrix by the homography; bad matrix size"));
  vpMatrix M(rowNum, 3);

  for (unsigned int i=0;i<rowNum;i++){
    for (unsigned int j=0;j<3;j++) {
      double s = 0;
      for (unsigned int k=0;k<3;k++) s += (*this)[i][k] * H[k][j];
      M[i][j] = s;
    }
  }

  return M;
}

void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C){
  try 
  {
    if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if ((A.colNum != B.getCols())||(A.rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\t vpMatrix mismatch in "
      "vpMatrix/vpMatrix addition")) ;
  }

  double ** ArowPtrs=A.rowPtrs;
  double ** BrowPtrs=B.rowPtrs;
  double ** CrowPtrs=C.rowPtrs;

  for (unsigned int i=0;i<A.rowNum;i++)
    for(unsigned int j=0;j<A.colNum;j++)     
      CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];

}

void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{  
  try 
  {
    if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if ((A.colNum != B.getCols())||(A.rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix addition") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\t vpMatrix mismatch in "
      "vpMatrix/vpMatrix addition")) ;
  }

  double ** ArowPtrs=A.rowPtrs;
  double ** BrowPtrs=B.rowPtrs;
  double ** CrowPtrs=C.rowPtrs;

  //    double t0,t1;
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<A.dsize;i++)
  //    {
  //        *(C.data + i) = *(B.data + i) + *(A.data + i) ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< std::endl;

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<A.rowNum;i++)
    for(unsigned int j=0;j<A.colNum;j++)
    {

      CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
    }
    //  t1=vpTime::measureTimeMicros();
    //  std::cout<< t1-t0<<" ";


}

vpMatrix vpMatrix::operator+(const vpMatrix &B) const
{

  vpMatrix C;
  vpMatrix::add2Matrices(*this,B,C);
  return C;
}


void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
  try 
  {
    if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix/vpMatrix substraction") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\t vpMatrix mismatch in "
      "vpMatrix/vpMatrix substraction")) ;
  }



  double ** ArowPtrs=A.rowPtrs;
  double ** BrowPtrs=B.rowPtrs;
  double ** CrowPtrs=C.rowPtrs;

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<A.dsize;i++)
  //    {
  //        *(C.data + i) = *(A.data + i) - *(B.data + i)   ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< std::endl;
  //    
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<A.rowNum;i++)
    for(unsigned int j=0;j<A.colNum;j++)
    {
      CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
    }
    //  t1=vpTime::measureTimeMicros();
    //  std::cout<< t1-t0<<" ";


}

vpMatrix vpMatrix::operator-(const vpMatrix &B) const
{
  vpMatrix C;
  vpMatrix::sub2Matrices(*this,B,C);
  return C;
}


vpMatrix &vpMatrix::operator+=(const vpMatrix &B)
{
  if ( (colNum != B.getCols())||(rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix +=  addition") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\t vpMatrix mismatch in "
      "vpMatrix += addition")) ;

  }


  double ** BrowPtrs=B.rowPtrs;

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //    {
  //        *(data + i) += *(B.data + i) ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< std::endl;

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)  
      rowPtrs[i][j] += BrowPtrs[i][j];
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";





  return *this;
}


vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
{
  if ( (colNum != B.getCols())||(rowNum != B.getRows()))
  {
    vpERROR_TRACE("\n\t\t vpMatrix mismatch in vpMatrix -= substraction") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "\n\t\t vpMatrix mismatch in "
      "vpMatrix -= substraction")) ;

  }

  double ** BrowPtrs=B.rowPtrs;

  //    double t0,t1;

  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //    {
  //        *(data + i) -= *(B.data + i) ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< " " ;

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j] -= BrowPtrs[i][j];

  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;



  return *this;
}

void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C)
{
  try 
  {
    if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  double ** ArowPtrs=A.rowPtrs;
  double ** CrowPtrs=C.rowPtrs;


  //    std::cout << "negate" << std::endl;
  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<A.dsize;i++)
  //    {
  //        *(C.data + i) = -*(A.data + i) ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<< " ";

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<A.rowNum;i++)
    for(unsigned int j=0;j<A.colNum;j++)
      CrowPtrs[i][j]= -ArowPtrs[i][j];
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;


}

vpMatrix vpMatrix::operator-() const //negate
{
  vpMatrix C;
  vpMatrix::negateMatrix(*this,C);
  return C;
}

double
vpMatrix::sumSquare() const
{
  double sum_square=0.0;
  double x ;

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    double *d = data ;
  //    double *n = data+dsize ;
  //    while (d < n )
  //    {
  //      x = *d++ ;
  //      sum_square += x*x ;
  //    }
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" "<< sum << " ";
  //    


  //    sum_square= 0.0;
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
    {
      x=rowPtrs[i][j];
      sum_square += x*x;
    }
    //  t1=vpTime::measureTimeMicros();
    //  std::cout<< t1-t0<<" "<< sum << std::endl;

    return sum_square;
}

double
vpMatrix::sum() const
{
  double s=0.0;
  for (unsigned int i=0;i<rowNum;i++)
  {
    for(unsigned int j=0;j<colNum;j++)
    {
      s += rowPtrs[i][j];
    }
  }

  return s;
}


//---------------------------------
// Matrix/vector operations.
//---------------------------------

void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &b, vpColVector &c)
{
  if (A.colNum != b.getRows())
  {
    vpERROR_TRACE("vpMatrix mismatch in vpMatrix/vector multiply") ;
    throw(vpMatrixException::incorrectMatrixSizeError) ;
  }

  try 
  {
    if (A.rowNum != c.rowNum) c.resize(A.rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }

  c = 0.0;
  for (unsigned int j=0;j<A.colNum;j++) 
  {
    double bj = b[j] ; // optimization em 5/12/2006
    for (unsigned int i=0;i<A.rowNum;i++) 
    {
      c[i]+=A.rowPtrs[i][j] * bj;
    }
  }
}

vpColVector
vpMatrix::operator*(const vpColVector &b) const
{
  vpColVector c;
  vpMatrix::multMatrixVector(*this,b,c);
  return c;
}

vpTranslationVector
vpMatrix::operator*(const vpTranslationVector &b) const
{
  vpTranslationVector c;

  if (rowNum != 3 || colNum != 3)
  {
    vpERROR_TRACE("vpMatrix mismatch in vpMatrix::operator*(const vpTranslationVector)") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "vpMatrix mismatch in vpMatrix::operator*()")) ;
  }

  for (unsigned int j=0;j<3;j++) c[j]=0 ;

  for (unsigned int j=0;j<3;j++) {
    {
      double bj = b[j] ; // optimization em 5/12/2006
      for (unsigned int i=0;i<3;i++) {
        c[i]+=rowPtrs[i][j] * bj;
      }
    }
  }
  return c ;
}

//---------------------------------
// Matrix/real operations.
//---------------------------------

vpMatrix operator*(const double &x,const vpMatrix &B)
{
  // Modif EM 13/6/03
  vpMatrix C ;

  try {
    C.resize(B.getRows(),B.getCols());
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }
  //    double t0,t1;

  unsigned int Brow = B.getRows() ;
  unsigned int Bcol = B.getCols() ;
  //    t0=vpTime::measureTimeMicros();
  // 
  //    for (int i=0;i<Brow; i++)
  //    {
  //        double *ci = C[i] ;
  //        double *Bi = B[i] ;
  //        for (int j=0 ; j < Bcol;j++)
  //        ci[j] = Bi[j]*x;
  //     }
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<Brow;i++)
    for(unsigned int j=0;j<Bcol;j++)
      C[i][j]= B[i][j]*x;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;



  return C ;
}

vpMatrix vpMatrix::operator*(double x) const
{
  vpMatrix C;

  try {
    C.resize(rowNum,colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    std::cout << me << std::endl ;
    throw ;
  }
  //    double t0,t1;

  double ** CrowPtrs=C.rowPtrs;

  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //      *(C.data+i) = *(data+i)*x;
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";
  //    
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      CrowPtrs[i][j]= rowPtrs[i][j]*x;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;

  return C;
}

vpMatrix  vpMatrix::operator/(double x) const
{
  vpMatrix C;

  try {
    C.resize(rowNum,colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  //if (x == 0) {
  if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
    //vpERROR_TRACE("Divide by zero in method /(double x)") ;
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /(double x)");
  }

  double  xinv = 1/x ;


  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //        *(C.data+i) = *(data+i)*xinv ;
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";
  //    
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      C[i][j]=rowPtrs[i][j]*xinv;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;

  return C;

}


vpMatrix & vpMatrix::operator+=(double x)
{

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //        *(data+i) += x;
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";
  //    
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j]+=x;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;

  return *this;
}


vpMatrix & vpMatrix::operator-=(double x)
{


  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //        *(data+i) -= x;
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";
  //    
  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j]-=x;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;

  return *this;
}

vpMatrix & vpMatrix::operator*=(double x)
{


  //   for (int i=0;i<dsize;i++)
  //     *(data+i) *= x;

  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j]*=x;

  return *this;
}

vpMatrix & vpMatrix::operator/=(double x)
{
  //if (x == 0)
  if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) 
    throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double x)");

  double xinv = 1/x ;

  //    double t0,t1;
  //      
  //    t0=vpTime::measureTimeMicros();
  //    for (int i=0;i<dsize;i++)
  //      *(data+i) *= xinv;
  //  
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<" ";

  //    t0=vpTime::measureTimeMicros();
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j]*=xinv;
  //    t1=vpTime::measureTimeMicros();
  //    std::cout<< t1-t0<<std::endl;

  return *this;
}

//----------------------------------------------------------------
// Matrix Operation
//----------------------------------------------------------------


void
vpMatrix::setIdentity(const double & val)
{
  if (rowNum != colNum)
  {
    vpERROR_TRACE("non square matrix") ;
    throw(vpMatrixException(vpMatrixException::matrixError)) ;
  }

  unsigned int i,j;
  for (i=0;i<rowNum;i++)
    for (j=0;j<colNum;j++)
      if (i==j) (*this)[i][j] = val ; else (*this)[i][j] = 0;
}

void
vpMatrix::eye(unsigned int n)
{
  try {
    eye(n,n) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }
}
void
vpMatrix::eye(unsigned int m, unsigned int n)
{
  try {
    resize(m,n) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }


  for (unsigned int i=0; i<rowNum; i++)
    for (unsigned int j=0; j<colNum; j++)
      if (i == j) (*this)[i][j] = 1;
      else        (*this)[i][j] = 0;

}


vpMatrix vpMatrix::t() const
{
  vpMatrix At ;

  try {
    At.resize(colNum,rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  unsigned int i,j;
  for (i=0;i<rowNum;i++)
  {
    double *coli = (*this)[i] ;
    for (j=0;j<colNum;j++)
      At[j][i] = coli[j];
  }
  return At;
}


vpMatrix vpMatrix::transpose()const
{
  vpMatrix At ;
  transpose(At);
  return At;
}

void  vpMatrix::transpose(vpMatrix & At )const{

  try {
    At.resize(colNum,rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  size_t A_step = colNum;
  double ** AtRowPtrs = At.rowPtrs;

  for( unsigned int i = 0; i < colNum; i++ )
  {
    double * row_ = AtRowPtrs[i];
    double * col = rowPtrs[0]+i;
    for( unsigned int j = 0; j < rowNum; j++, col+=A_step )
      *(row_++)=*col;
  }
}


vpMatrix vpMatrix::AAt() const
{
  vpMatrix B;

  AAt(B);

  return B;
}

void vpMatrix::AAt(vpMatrix &B)const {

  try {
    if ((B.rowNum != rowNum) || (B.colNum != rowNum)) B.resize(rowNum,rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  // compute A*A^T
  for(unsigned int i=0;i<rowNum;i++){
    for(unsigned int j=i;j<rowNum;j++){
      double *pi = rowPtrs[i];// row i
      double *pj = rowPtrs[j];// row j

      // sum (row i .* row j)
      double ssum=0;
      for(unsigned int k=0; k < colNum ;k++) 
        ssum += *(pi++)* *(pj++);

      B[i][j]=ssum; //upper triangle
      if(i!=j)
        B[j][i]=ssum; //lower triangle
    }
  }
}



void vpMatrix::AtA(vpMatrix &B) const
{
  try {
    if ((B.rowNum != colNum) || (B.colNum != colNum)) B.resize(colNum,colNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  unsigned int i,j,k;
  double s;
  double *ptr;
  double *Bi;
  for (i=0;i<colNum;i++)
  {
    Bi = B[i] ;
    for (j=0;j<i;j++)
    {
      ptr=data;
      s = 0 ;
      for (k=0;k<rowNum;k++)
      {
        s +=(*(ptr+i)) * (*(ptr+j));
        ptr+=colNum;
      }
      *Bi++ = s ;
      B[j][i] = s;
    }
    ptr=data;
    s = 0 ;
    for (k=0;k<rowNum;k++)
    {
      s +=(*(ptr+i)) * (*(ptr+i));
      ptr+=colNum;
    }
    *Bi = s;
  }
}


vpMatrix vpMatrix::AtA() const
{
  vpMatrix B;

  AtA(B);

  return B;
}


void vpMatrix::stackColumns(vpColVector  &out ){

  try {
    if ((out.rowNum != colNum*rowNum) || (out.colNum != 1)) out.resize(rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  double *optr=out.data;
  for(unsigned int j =0;j<colNum ; j++){
    for(unsigned int i =0;i<rowNum ; i++){
      *(optr++)=rowPtrs[i][j];
    }
  }
}

vpColVector vpMatrix::stackColumns(){

  vpColVector out(colNum*rowNum);
  stackColumns(out);
  return out;
}

void vpMatrix::stackRows(vpRowVector  &out ){

  try {
    if ((out.rowNum != 1) || (out.colNum != colNum*rowNum)) out.resize(rowNum);
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }

  double *mdata=data;
  double *optr=out.data;
  for(unsigned int i =0;i<dsize ; i++){
    *(optr++)=*(mdata++);
  }
}
vpRowVector vpMatrix::stackRows(){

  vpRowVector out(colNum*rowNum);
  stackRows(out );
  return out; 
}

void vpMatrix::kron(const vpMatrix  &m1, const vpMatrix  &m2 , vpMatrix  &out){
  unsigned int r1= m1.getRows();
  unsigned int c1= m1.getCols();
  unsigned int r2= m2.getRows();
  unsigned int c2= m2.getCols();


  if (r1*r2 !=out.rowNum || c1*c2!= out.colNum )
  {
    vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Kronecker prodect bad dimension of output vpMatrix"));
  }

  for(unsigned int r =0;r<r1 ; r++){
    for(unsigned int c =0;c<c1 ; c++){
      double alpha = m1[r][c];
      double *m2ptr = m2[0];
      unsigned int roffset= r*r2;
      unsigned int coffset= c*c2;
      for(unsigned int rr =0;rr<r2 ; rr++){
        for(unsigned int cc =0;cc<c2 ;cc++){
          out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
        }
      }
    }
  }

}

void vpMatrix::kron(const vpMatrix  &m , vpMatrix  &out){
  kron(*this,m,out);
}

vpMatrix vpMatrix::kron(const vpMatrix  &m1, const vpMatrix  &m2 ){

  unsigned int r1= m1.getRows();
  unsigned int c1= m1.getCols();
  unsigned int r2= m2.getRows();
  unsigned int c2= m2.getCols();

  vpMatrix out(r1*r2,c1*c2);

  for(unsigned int r =0;r<r1 ; r++){
    for(unsigned int c =0;c<c1 ; c++){
      double alpha = m1[r][c];
      double *m2ptr = m2[0];
      unsigned int roffset= r*r2;
      unsigned int coffset= c*c2;
      for(unsigned int rr =0;rr<r2 ; rr++){
        for(unsigned int cc =0;cc<c2 ;cc++){
          out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
        }
      }
    }
  }
  return out;
}


vpMatrix vpMatrix::kron(const vpMatrix  &m){
  return kron(*this,m);
}

void
vpMatrix::solveBySVD(const vpColVector &b, vpColVector &x) const
{
  x = pseudoInverse(1e-6)*b ;
}


vpColVector vpMatrix::solveBySVD(const vpColVector &B) const
{
  vpColVector X(colNum);

  solveBySVD(B, X);
  return X;
}


void
vpMatrix::svd(vpColVector& w, vpMatrix& v)
{
#if (DEBUG_LEVEL1 == 0) /* no verification */
  {
    w.resize( this->getCols() );
    v.resize( this->getCols(), this->getCols() );

#if defined (VISP_HAVE_LAPACK)
    svdLapack(w,v);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
    svdOpenCV(w,v);
#elif defined (VISP_HAVE_GSL)  /* be careful of the copy below */
    svdGsl(w,v) ;
#else
    svdNr(w,v) ;
#endif

    //svdNr(w,v) ;
  }
#else  /* verification of the SVD */
  {
    int pb = 0;
    unsigned int i,j,k,nrows,ncols;
    vpMatrix A, Asvd;

    A = (*this);        /* copy because svd is destructive */

    w.resize( this->getCols() );
    v.resize( this->getCols(), this->getCols() );
#ifdef VISP_HAVE_GSL  /* be careful of the copy above */
    svdGsl(w,v) ;
#else
    svdNr(w,v) ;
#endif
    //svdNr(w,v) ;

    nrows = A.getRows();
    ncols = A.getCols();
    Asvd.resize(nrows,ncols);

    for (i = 0 ; i < nrows ; i++)
    {
      for (j = 0 ; j < ncols ; j++)
      {
        Asvd[i][j] = 0.0;
        for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*this)[i][k]*w[k]*v[j][k];
      }
    }
    for (i=0;i<nrows;i++)
    {
      for (j=0;j<ncols;j++) if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
    }
    if (pb == 1)
    {
      printf("pb in SVD\n");
      std::cout << " A : " << std::endl << A << std::endl;
      std::cout << " Asvd : " << std::endl << Asvd << std::endl;
    }
    //    else printf("SVD ok ;-)\n");  /* It's so good... */
  }
#endif
}
unsigned int
vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
{
  vpColVector sv ;
  return   pseudoInverse(Ap, sv, svThreshold) ;
}

vpMatrix
vpMatrix::pseudoInverse(double svThreshold) const
{
  vpMatrix Ap ;
  vpColVector sv ;
  pseudoInverse(Ap, sv, svThreshold) ;
  return   Ap ;
}

unsigned int
vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  vpMatrix imA, imAt ;
  return pseudoInverse(Ap, sv, svThreshold, imA, imAt) ;
}

unsigned int 
vpMatrix::pseudoInverse(vpMatrix &Ap,
                        vpColVector &sv, double svThreshold,
                        vpMatrix &imA,
                        vpMatrix &imAt) const
{

  unsigned int i, j, k ;

  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows() ;
  unsigned int ncols_orig = getCols() ;
  Ap.resize(ncols_orig,nrows_orig) ;

  if (nrows_orig >=  ncols_orig)
  {
    nrows = nrows_orig;
    ncols = ncols_orig;
  }
  else
  {
    nrows = ncols_orig;
    ncols = nrows_orig;
  }

  vpMatrix a(nrows,ncols) ;
  vpMatrix a1(ncols,nrows);
  vpMatrix v(ncols,ncols) ;
  sv.resize(ncols) ;

  if (nrows_orig >=  ncols_orig) a = *this;
  else a = (*this).t();

  a.svd(sv,v);

  // compute the highest singular value and the rank of h
  double maxsv = 0 ;
  for (i=0 ; i < ncols ; i++)
    if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;

  unsigned int rank = 0 ;
  for (i=0 ; i < ncols ; i++)
    if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;



  /*------------------------------------------------------- */
  for (i = 0 ; i < ncols ; i++)
  {
    for (j = 0 ; j < nrows ; j++)
    {
      a1[i][j] = 0.0;

      for (k=0 ; k < ncols ; k++)
        if (fabs(sv[k]) > maxsv*svThreshold)
        {
          a1[i][j] += v[i][k]*a[j][k]/sv[k];
        }
    }
  }
  if (nrows_orig >=  ncols_orig) Ap = a1;
  else Ap = a1.t();

  if (nrows_orig >=  ncols_orig)
  {
    //  compute dim At
    imAt.resize(ncols_orig,rank) ;
    for (i=0 ; i  < ncols_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imAt[i][j] = v[i][j] ;

    //  compute dim A
    imA.resize(nrows_orig,rank) ;
    for (i=0 ; i  < nrows_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imA[i][j] = a[i][j] ;
  }
  else
  {
    //  compute dim At
    imAt.resize(ncols_orig,rank) ;
    for (i=0 ; i  < ncols_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imAt[i][j] = a[i][j] ;

    imA.resize(nrows_orig,rank) ;
    for (i=0 ; i  < nrows_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imA[i][j] = v[i][j] ;

  }

#if (DEBUG_LEVEL1)
  {
    int pb = 0;
    vpMatrix A, ApA, AAp, AApA, ApAAp ;

    nrows = nrows_orig;
    ncols = ncols_orig;

    A.resize(nrows,ncols) ;
    A = *this ;

    ApA = Ap * A;
    AApA = A * ApA;
    ApAAp = ApA * Ap;
    AAp = A * Ap;

    for (i=0;i<nrows;i++)
    {
      for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
    }
    for (i=0;i<ncols;i++)
    {
      for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
    }
    for (i=0;i<nrows;i++)
    {
      for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
    }
    for (i=0;i<ncols;i++)
    {
      for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
    }
    if (pb == 1)
    {
      printf("pb in pseudo inverse\n");
      std::cout << " A : " << std::endl << A << std::endl;
      std::cout << " Ap : " << std::endl << Ap << std::endl;
      std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
      std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
      std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
      std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
    }
    //    else printf("Ap OK ;-) \n");

  }
#endif

  // std::cout << v << std::endl ;
  return rank ;
}



unsigned int 
vpMatrix::pseudoInverse(vpMatrix &Ap,
                        vpColVector &sv, double svThreshold,
                        vpMatrix &imA,
                        vpMatrix &imAt,
                        vpMatrix &kerA) const
{

  unsigned int i, j, k ;

  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows() ;
  unsigned int ncols_orig = getCols() ;
  Ap.resize(ncols_orig,nrows_orig) ;

  if (nrows_orig >=  ncols_orig)
  {
    nrows = nrows_orig;
    ncols = ncols_orig;
  }
  else
  {
    nrows = ncols_orig;
    ncols = nrows_orig;
  }

  vpMatrix a(nrows,ncols) ;
  vpMatrix a1(ncols,nrows);
  vpMatrix v(ncols,ncols) ;
  sv.resize(ncols) ;

  if (nrows_orig >=  ncols_orig) a = *this;
  else a = (*this).t();

  a.svd(sv,v);

  // compute the highest singular value and the rank of h
  double maxsv = 0 ;
  for (i=0 ; i < ncols ; i++)
    if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;

  unsigned int rank = 0 ;
  for (i=0 ; i < ncols ; i++)
    if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;



  /*------------------------------------------------------- */
  for (i = 0 ; i < ncols ; i++)
  {
    for (j = 0 ; j < nrows ; j++)
    {
      a1[i][j] = 0.0;

      for (k=0 ; k < ncols ; k++)
        if (fabs(sv[k]) > maxsv*svThreshold)
        {
          a1[i][j] += v[i][k]*a[j][k]/sv[k];
        }
    }
  }
  if (nrows_orig >=  ncols_orig) Ap = a1;
  else Ap = a1.t();

  if (nrows_orig >=  ncols_orig)
  {
    //  compute dim At
    imAt.resize(ncols_orig,rank) ;
    for (i=0 ; i  < ncols_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imAt[i][j] = v[i][j] ;

    //  compute dim A
    imA.resize(nrows_orig,rank) ;
    for (i=0 ; i  < nrows_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imA[i][j] = a[i][j] ;
  }
  else
  {
    //  compute dim At
    imAt.resize(ncols_orig,rank) ;
    for (i=0 ; i  < ncols_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imAt[i][j] = a[i][j] ;

    imA.resize(nrows_orig,rank) ;
    for (i=0 ; i  < nrows_orig ; i++)
      for (j=0 ; j < rank ; j++)
        imA[i][j] = v[i][j] ;

  }

  vpMatrix cons(ncols_orig, ncols_orig);
  cons = 0;

  for (j = 0; j < ncols_orig; j++)
  {
    for (i = 0; i < ncols_orig; i++)
    {
      if (fabs(sv[i]) <= maxsv*svThreshold)
      {
        cons[i][j] = v[j][i];
      }
    }
  }

  vpMatrix Ker (ncols_orig-rank, ncols_orig);
  k = 0;
  for (j = 0; j < ncols_orig ; j++)
  {
    //if ( cons.row(j+1).sumSquare() != 0)
    if ( std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
    {
      for (i = 0; i < cons.getCols(); i++)
        Ker[k][i] = cons[j][i];

      k++;
    }
  }
  kerA = Ker;

#if (DEBUG_LEVEL1)
  {
    int pb = 0;
    vpMatrix A, ApA, AAp, AApA, ApAAp ;

    nrows = nrows_orig;
    ncols = ncols_orig;

    A.resize(nrows,ncols) ;
    A = *this ;

    ApA = Ap * A;
    AApA = A * ApA;
    ApAAp = ApA * Ap;
    AAp = A * Ap;

    for (i=0;i<nrows;i++)
    {
      for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
    }
    for (i=0;i<ncols;i++)
    {
      for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
    }
    for (i=0;i<nrows;i++)
    {
      for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
    }
    for (i=0;i<ncols;i++)
    {
      for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
    }
    if (pb == 1)
    {
      printf("pb in pseudo inverse\n");
      std::cout << " A : " << std::endl << A << std::endl;
      std::cout << " Ap : " << std::endl << Ap << std::endl;
      std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
      std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
      std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
      std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
      std::cout << " KerA : " << std::endl << kerA << std::endl;
    }
    //    else printf("Ap OK ;-) \n");

  }
#endif

  // std::cout << v << std::endl ;
  return rank ;
}

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

vpRowVector
vpMatrix::row(const unsigned int i)
{
  vpRowVector c(getCols()) ;

  for (unsigned int j =0 ; j < getCols() ; j++)  c[j] = (*this)[i-1][j] ;
  return c ;
}

vpColVector
vpMatrix::column(const unsigned int j)
{
  vpColVector c(getRows()) ;

  for (unsigned int i =0 ; i < getRows() ; i++)     c[i] = (*this)[i][j-1] ;
  return c ;
}
#endif


vpColVector
vpMatrix::getCol(const unsigned int j, const unsigned int i_begin, const unsigned int column_size) const
{
  if (i_begin + column_size > getRows() || j >= getCols())
    throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
  vpColVector c(column_size);
  for (unsigned int i=0 ; i < column_size ; i++)
    c[i] = (*this)[i_begin+i][j];
  return c;
}

vpColVector
vpMatrix::getCol(const unsigned int j) const
{
  if (j >= getCols())
    throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
  unsigned int nb_rows = getRows();
  vpColVector c(nb_rows);
  for (unsigned int i=0 ; i < nb_rows ; i++)
    c[i] = (*this)[i][j];
  return c;
}

vpRowVector
vpMatrix::getRow(const unsigned int i) const
{
  if (i >= getRows())
    throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
  unsigned int nb_cols = getCols();
  vpRowVector r( nb_cols );
  for (unsigned int j=0 ; j < nb_cols ; j++)
    r[j] = (*this)[i][j];
  return r;
}

vpRowVector
vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigned int row_size) const
{
  if (j_begin + row_size > getCols() || i >= getRows())
    throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
  vpRowVector r(row_size);
  for (unsigned int j=0 ; j < row_size ; j++)
    r[j] = (*this)[i][j_begin+i];
  return r;
}

vpMatrix
vpMatrix::stackMatrices(const vpMatrix &A, const vpMatrix &B)
{
  vpMatrix C ;

  try{
    stackMatrices(A,B, C) ;
  }
  catch(vpMatrixException me)
  {
    vpCERROR << me << std::endl;
    throw ;
  }

  return C ;
}

void
vpMatrix::stackMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
  unsigned int nra = A.getRows() ;
  unsigned int nrb = B.getRows() ;

  if (nra !=0)
    if (A.getCols() != B.getCols())
    {
      vpERROR_TRACE("\n\t\t incorrect matrices size") ;
      throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
        "\n\t\t incorrect matrices size")) ;
    }

    try {
      C.resize(nra+nrb,B.getCols()  ) ;
    }
    catch(vpException me)
    {
      vpERROR_TRACE("Error caught") ;
      vpCERROR << me << std::endl ;
      throw ;
    }

    unsigned int i,j ;
    for (i=0 ; i < nra ; i++)
      for (j=0 ; j < A.getCols() ; j++)
        C[i][j] = A[i][j] ;


    for (i=0 ; i < nrb ; i++)
      for (j=0 ; j < B.getCols() ; j++)
      {
        C[i+nra][j] = B[i][j] ;

      }


}





vpMatrix
vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, 
                 const unsigned int r, const unsigned int c)
{
  vpMatrix C ;

  try{
    insert(A,B, C, r, c) ;
  }
  catch(vpMatrixException me)
  {
    vpCERROR << me << std::endl;
    throw me;
  }

  return C ;
}

void
vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, 
                 const unsigned int r, const unsigned int c)
{
  if( ( (r + B.getRows()) <= A.getRows() ) && 
    ( (c + B.getCols()) <= A.getCols() ) ){
      try {
        C.resize(A.getRows(),A.getCols()  ) ;
      }
      catch(vpException me)
      {
        vpERROR_TRACE("Error caught") ;
        vpCERROR << me << std::endl ;
        throw ;
      }
      for(unsigned int i=0; i<A.getRows(); i++){
        for(unsigned int j=0; j<A.getCols(); j++){
          if(i >= r && i < (r + B.getRows()) && j >= c && j < (c+B.getCols())){
            C[i][j] = B[i-r][j-c];
          }
          else{
            C[i][j] = A[i][j];
          }
        }
      }
  }
  else{
    throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, 
      "\n\t\tIncorrect size of the matrix to insert data.");
  }
}

vpMatrix
vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
{
  vpMatrix C ;

  try{
    juxtaposeMatrices(A,B, C) ;
  }
  catch(vpMatrixException me)
  {
    vpCERROR << me << std::endl ;
    throw ;
  }

  return C ;
}

void
vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
  unsigned int nca = A.getCols() ;
  unsigned int ncb = B.getCols() ;

  if (nca !=0)
    if (A.getRows() != B.getRows())
    {
      vpERROR_TRACE("\n\t\t incorrect matrices size") ;
      throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
        "\n\t\t incorrect matrices size")) ;
    }

    try {
      C.resize(B.getRows(),nca+ncb) ;
    }
    catch(vpException me)
    {
      vpERROR_TRACE("Error caught") ;
      vpCERROR << me << std::endl ;
      throw ;
    }

    unsigned int i,j ;
    for (i=0 ; i < C.getRows(); i++)
      for (j=0 ; j < nca ; j++)
        C[i][j] = A[i][j] ;


    for (i=0 ; i < C.getRows() ; i++)
      for (j=0 ; j < ncb ; j++)
      {
        C[i][nca+j] = B[i][j] ;
      }
}

void
vpMatrix::diag(const vpColVector &A)
{
  unsigned int rows = A.getRows() ;
  try {
    this->resize(rows,rows) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }
  (*this) = 0 ;
  for (unsigned int i=0 ; i< rows ; i++ )
    (* this)[i][i] = A[i] ;
}
void
vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
{
  unsigned int rows = A.getRows() ;
  try {
    DA.resize(rows,rows) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("Error caught") ;
    vpCERROR << me << std::endl ;
    throw ;
  }
  DA =0 ;
  for (unsigned int i=0 ; i< rows ; i++ )
    DA[i][i] = A[i] ;
}

//--------------------------------------------------------------------
// Output
//--------------------------------------------------------------------


VISP_EXPORT std::ostream &operator <<(std::ostream &s,const vpMatrix &m)
{
  std::ios_base::fmtflags original_flags = s.flags();

  s.precision(10) ;
  for (unsigned int i=0;i<m.getRows();i++) {
    for (unsigned int j=0;j<m.getCols();j++){
      s <<  m[i][j] << "  ";
    }
    // We don't add a \n char on the end of the last matrix line
    if (i < m.getRows()-1)
      s << std::endl;
  }

  s.flags(original_flags); // restore s to standard state

  return s;
}

int
vpMatrix::print(std::ostream& s, unsigned int length, char const* intro)
{
  typedef std::string::size_type size_type;

  unsigned int m = getRows();
  unsigned int n = getCols();

  std::vector<std::string> values(m*n);
  std::ostringstream oss;
  std::ostringstream ossFixed;
  std::ios_base::fmtflags original_flags = oss.flags();

  // ossFixed <<std::fixed;
  ossFixed.setf ( std::ios::fixed, std::ios::floatfield );

  size_type maxBefore=0;  // the length of the integral part
  size_type maxAfter=0;   // number of decimals plus
  // one place for the decimal point
  for (unsigned int i=0;i<m;++i) {
    for (unsigned int j=0;j<n;++j){
      oss.str("");
      oss << (*this)[i][j];
      if (oss.str().find("e")!=std::string::npos){
        ossFixed.str("");
        ossFixed << (*this)[i][j];
        oss.str(ossFixed.str());
      }

      values[i*n+j]=oss.str();
      size_type thislen=values[i*n+j].size();
      size_type p=values[i*n+j].find('.');

      if (p==std::string::npos){
        maxBefore=vpMath::maximum(maxBefore, thislen);
        // maxAfter remains the same
      } else{
        maxBefore=vpMath::maximum(maxBefore, p);
        maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
      }
    }
  }

  size_type totalLength=length;
  // increase totalLength according to maxBefore
  totalLength=vpMath::maximum(totalLength,maxBefore);
  // decrease maxAfter according to totalLength
  maxAfter=std::min(maxAfter, totalLength-maxBefore);
  if (maxAfter==1) maxAfter=0;

  // the following line is useful for debugging
  //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";

  if (intro) s <<intro;
  s <<"["<<m<<","<<n<<"]=\n";

  for (unsigned int i=0;i<m;i++) {
    s <<"  ";
    for (unsigned int j=0;j<n;j++){
      size_type p=values[i*n+j].find('.');
      s.setf(std::ios::right, std::ios::adjustfield);
      s.width((std::streamsize)maxBefore);
      s <<values[i*n+j].substr(0,p).c_str();

      if (maxAfter>0){
        s.setf(std::ios::left, std::ios::adjustfield);
        if (p!=std::string::npos){
          s.width((std::streamsize)maxAfter);
          s <<values[i*n+j].substr(p,maxAfter).c_str();
        } else{
          assert(maxAfter>1);
          s.width((std::streamsize)maxAfter);
          s <<".0";
        }
      }

      s <<' ';
    }
    s <<std::endl;
  }

  s.flags(original_flags); // restore s to standard state

  return (int)(maxBefore+maxAfter);
}


std::ostream & vpMatrix::
matlabPrint(std::ostream & os) const
{

  unsigned int i,j;

  os << "[ ";
  for (i=0; i < this->getRows(); ++ i)
  {
    for (j=0; j < this ->getCols(); ++ j)
    {
      os <<  (*this)[i][j] << ", ";
    }
    if (this ->getRows() != i+1) { os << ";" << std::endl; }
    else { os << "]" << std::endl; }
  }
  return os;
};

std::ostream & vpMatrix::
maplePrint(std::ostream & os) const
{
  unsigned int i,j;

  os << "([ " << std::endl;
  for (i=0; i < this->getRows(); ++ i)
  { os << "[";
    for (j=0; j < this->getCols(); ++ j)
    {
      os <<  (*this)[i][j] << ", ";
    }
    os << "]," << std::endl;
  }
   os << "])" << std::endl;
  return os;
};

std::ostream & vpMatrix::
csvPrint(std::ostream & os) const
{
  unsigned int i,j;

  for (i=0; i < this->getRows(); ++ i)
  {
    for (j=0; j < this->getCols(); ++ j)
    {
      os <<  (*this)[i][j];
      if (!(j==(this->getCols()-1)))
        os << ", ";
    }
    os << std::endl;
  }
  return os;
};


std::ostream & vpMatrix::
cppPrint(std::ostream & os, const char * matrixName, bool octet) const
{

  unsigned int i,j;
  const char defaultName [] = "A";
  if (NULL == matrixName)
  {
    matrixName = defaultName;
  }
  os << "vpMatrix " << defaultName
    << " (" << this ->getRows ()
    << ", " << this ->getCols () << "); " <<std::endl;

  for (i=0; i < this->getRows(); ++ i)
  {
    for (j=0; j < this ->getCols(); ++ j)
    {
      if (! octet)
      {
        os << defaultName << "[" << i << "][" << j
          << "] = " << (*this)[i][j] << "; " << std::endl;
      }
      else
      {
        for (unsigned int k = 0; k < sizeof(double); ++ k)
        {
          os << "((unsigned char*)&(" << defaultName
            << "[" << i << "][" << j << "]) )[" << k
            <<"] = 0x" <<std::hex<<
            (unsigned int)((unsigned char*)& ((*this)[i][j])) [k]
          << "; " << std::endl;
        }
      }
    }
    os << std::endl;
  }
  return os;
};


double
vpMatrix::euclideanNorm () const
{
  double norm=0.0;
  double x ;
  for (unsigned int i=0;i<dsize;i++) {
    x = *(data +i); norm += x*x; 
  }

  return sqrt(norm);
}



double
vpMatrix::infinityNorm () const
{
  double norm=0.0;
  double x ;
  for (unsigned int i=0;i<rowNum;i++){
    x = 0;
    for (unsigned int j=0; j<colNum;j++){
      x += fabs (*(*(rowPtrs + i)+j)) ;
    }
    if (x > norm) {
      norm = x;
    }
  }
  return norm;
}

double vpMatrix::detByLU() const
{
  double det_ = 0;

  // Test wether the matrix is squred
  if (rowNum == colNum)
  {
    // create a temporary matrix that will be modified by LUDcmp
    vpMatrix tmp(*this);

    // using th LUdcmp based on NR codes
    // it modified the tmp matrix in a special structure of type :
    //  b11 b12 b13 b14
    //  a21 b22 b23 b24
    //  a21 a32 b33 b34
    //  a31 a42 a43 b44 

    unsigned int  * perm = new unsigned int[rowNum];  // stores the permutations
    int d;   // +- 1 fi the number of column interchange is even or odd
    tmp.LUDcmp(perm,  d);
    delete[]perm;

    // compute the determinant that is the product of the eigen values
    det_ = (double) d;
    for(unsigned int i=0;i<rowNum;i++)
    {
      det_*=tmp[i][i];
    }
  }

  else {
    vpERROR_TRACE("Determinant Computation : ERR Matrix not squared") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
      "\n\t\tDeterminant Computation : ERR Matrix not squared")) ;


  }
  return det_ ;
}



void vpMatrix::stackMatrices(const vpMatrix &A)
{
  if(rowNum == 0)
    *this = A;
  else
    *this = vpMatrix::stackMatrices(*this, A);
}


void vpMatrix::insert(const vpMatrix&A, const unsigned int r, 
                      const unsigned int c)
{
  if( (r + A.getRows() ) <= rowNum && (c + A.getCols() ) <= colNum ){
    // recopy matrix A in the current one, does not call static function to avoid initialisation and recopy of matrix
    for(unsigned int i=r; i<(r+A.getRows()); i++){
      for(unsigned int j=c; j<(c+A.getCols()); j++){
        (*this)[i][j] = A[i-r][j-c];
      }
    }
  }
  else{
    throw vpMatrixException(vpMatrixException::incorrectMatrixSizeError, 
      "\n\t\tIncorrect size of the matrix to insert data.");
  }
}


vpColVector vpMatrix::eigenValues()
{
  if (rowNum != colNum) {
    vpERROR_TRACE("Eigen values computation: ERR Matrix not square") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
      "\n\t\tEigen values computation: ERR Matrix not square")) ;
  }

#ifdef VISP_HAVE_GSL  /* be careful of the copy below */
  {
    // Check if the matrix is symetric: At - A = 0
    vpMatrix At_A = (*this).t() - (*this);
    for (unsigned int i=0; i < rowNum; i++) {
      for (unsigned int j=0; j < rowNum; j++) {
        //if (At_A[i][j] != 0) {
        if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
          vpERROR_TRACE("Eigen values computation: ERR Matrix not symmetric") ;
          throw(vpMatrixException(vpMatrixException::matrixError,
            "\n\t\tEigen values computation: "
            "ERR Matrix not symmetric")) ;
        }
      }
    }


    vpColVector evalue(rowNum); // Eigen values

    gsl_vector *eval = gsl_vector_alloc (rowNum);
    gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);

    gsl_eigen_symmv_workspace * w =  gsl_eigen_symmv_alloc (rowNum);
    gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);

    unsigned int Atda = m->tda ;
    for (unsigned int i=0 ; i < rowNum ; i++){
      unsigned int k = i*Atda ;
      for (unsigned int j=0 ; j < colNum ; j++)
        m->data[k+j] = (*this)[i][j] ;
    }
    gsl_eigen_symmv (m, eval, evec, w);

    gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);

    for (unsigned int i=0; i < rowNum; i++) {
      evalue[i] = gsl_vector_get (eval, i);
    }

    gsl_eigen_symmv_free (w);
    gsl_vector_free (eval);
    gsl_matrix_free (m);
    gsl_matrix_free (evec);

    return evalue;
  }
#else
  {
    vpERROR_TRACE("Not implemented since the GSL library is not detected.") ;
    throw(vpMatrixException(vpMatrixException::notImplementedError,
      "\n\t\tEigen values Computation: Not implemented "
      "since the GSL library is not detected")) ;
  }
#endif  
}

#ifdef VISP_HAVE_GSL  /* be careful of the copy below */
void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector)
#else
void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */)
#endif
{
  if (rowNum != colNum) {
    vpERROR_TRACE("Eigen values computation: ERR Matrix not square") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
      "\n\t\tEigen values computation: ERR Matrix not square")) ;
  }

#ifdef VISP_HAVE_GSL  /* be careful of the copy below */
  {
    // Check if the matrix is symetric: At - A = 0
    vpMatrix At_A = (*this).t() - (*this);
    for (unsigned int i=0; i < rowNum; i++) {
      for (unsigned int j=0; j < rowNum; j++) {
        //if (At_A[i][j] != 0) {
        if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
          vpERROR_TRACE("Eigen values computation: ERR Matrix not symmetric") ;
          throw(vpMatrixException(vpMatrixException::matrixError,
            "\n\t\tEigen values computation: "
            "ERR Matrix not symmetric")) ;
        }
      }
    }

    // Resize the output matrices
    evalue.resize(rowNum);
    evector.resize(rowNum, colNum);

    gsl_vector *eval = gsl_vector_alloc (rowNum);
    gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);

    gsl_eigen_symmv_workspace * w =  gsl_eigen_symmv_alloc (rowNum);
    gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);

    unsigned int Atda = m->tda ;
    for (unsigned int i=0 ; i < rowNum ; i++){
      unsigned int k = i*Atda ;
      for (unsigned int j=0 ; j < colNum ; j++)
        m->data[k+j] = (*this)[i][j] ;
    }
    gsl_eigen_symmv (m, eval, evec, w);

    gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);

    for (unsigned int i=0; i < rowNum; i++) {
      evalue[i] = gsl_vector_get (eval, i);
    }
    Atda = evec->tda ;
    for (unsigned int i=0; i < rowNum; i++) {
      unsigned int k = i*Atda ;
      for (unsigned int j=0; j < rowNum; j++) {
        evector[i][j] = evec->data[k+j];
      }
    }


    //        {
    //          int i;

    //          for (i = 0; i < rowNum; i++)
    //            {
    //              double eval_i 
    //                 = gsl_vector_get (eval, i);
    //              gsl_vector_view evec_i 
    //                 = gsl_matrix_column (evec, i);

    //              printf ("eigenvalue = %g\n", eval_i);
    //              printf ("eigenvector = \n");
    //              gsl_vector_fprintf (stdout, 
    //                                  &evec_i.vector, "%g");
    //            }
    //        }

    gsl_eigen_symmv_free (w);
    gsl_vector_free (eval);
    gsl_matrix_free (m);
    gsl_matrix_free (evec);
  }
#else
  {
    vpERROR_TRACE("Not implemented since the GSL library is not detected.") ;
    throw(vpMatrixException(vpMatrixException::notImplementedError,
      "\n\t\tEigen values Computation: Not implemented "
      "since the GSL library is not detected")) ;
  }
#endif  
}


unsigned int 
vpMatrix::kernel(vpMatrix &kerA, double svThreshold)
{
#if (DEBUG_LEVEL1)
  std::cout << "begin Kernel" << std::endl ;
#endif
  unsigned int i, j ;
  unsigned int ncaptor = getRows() ;
  unsigned int ddl = getCols() ;
  vpMatrix C ;

  if (ncaptor == 0)
    std::cout << "Erreur Matrice  non initialise" << std::endl ;

#if (DEBUG_LEVEL2)
  {
    std::cout << "Interaction matrix L" << std::endl ;
    std::cout << *this   ;
    std::cout << "signaux capteurs : " << ncaptor << std::endl ;
  }
#endif

  C.resize(ddl,ncaptor) ;
  unsigned int min ;

  if (ncaptor > ddl) min = ddl ; else min = ncaptor ;

  // ! the SVDcmp function inthe matrix lib is destructive

  vpMatrix a1 ;
  vpMatrix a2 ;

  vpColVector sv(ddl) ;   // singular values
  vpMatrix v(ddl,ddl) ;

  if (ncaptor < ddl)
  {
    a1.resize(ddl,ddl) ;
  }
  else
  {
    a1.resize(ncaptor,ddl) ;
  }

  a2.resize(ncaptor,ddl) ;

  for (i=0 ; i < ncaptor ; i++)
    for (j=0 ; j < ddl ; j++)
    {
      a1[i][j] = (*this)[i][j] ;
      a2[i][j] = (*this)[i][j] ;
    }


    if (ncaptor < ddl)
    {
      for (i=ncaptor ; i < ddl ; i++)
        for (j=0 ; j < ddl ; j++)
        {
          a1[i][j] = 0 ;
        }
        a1.svd(sv,v);
    }
    else
    {
      a1.svd(sv,v);
    }

    // compute the highest singular value and the rank of h

    double maxsv = 0 ;
    for (i=0 ; i < ddl ; i++)
      if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;

#if (DEBUG_LEVEL2)
    {
      std::cout << "Singular Value : (" ;
      for (i=0 ; i < ddl ; i++) std::cout << sv[i] << " , " ;
      std::cout << ")" << std::endl ;
    }
#endif

    unsigned int rank = 0 ;
    for (i=0 ; i < ddl ; i++)
      if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;

#if (DEBUG_LEVEL2)
    {

      for (i = 0 ; i < ddl ; i++)
      {
        for (j = 0 ; j < ncaptor ; j++)
        {
          unsigned int k=0 ;
          C[i][j] = 0.0;

          // modif le 25 janvier 1999 0.001 <-- maxsv*1.e-ndof
          // sinon on peut observer une perte de range de la matrice
          // ( d'ou venait ce 0.001 ??? )
          for (k=0 ; k < ddl ; k++)  if (fabs(sv[k]) > maxsv*svThreshold)
          {
            C[i][j] += v[i][k]*a1[j][k]/sv[k];
          }
        }
      }

      // cout << v << endl ;
      std::cout << C << std::endl ;
    }
#endif
    /*------------------------------------------------------- */
    if (rank != ddl)
    {
      // Compute the kernel if wanted
      if (min < ddl)
      {
        vpMatrix ch(ddl,ddl) ;
        ch = C*a2 ;
        ch.svd(sv,v) ;

      }
      //   if (noyau == 1)
      {
        maxsv = 0 ;
        for (i=0 ; i < ddl ; i++)
          if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
        rank = 0 ;
        for (i=0 ; i < ddl ; i++)
          if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
        vpMatrix cons(ddl,ddl) ;

        cons =0 ;
        for (j = 0 ; j < ddl ; j++)
        {
          for (i = 0 ; i < ddl ; i++)
            // Change Nicolas Mansard 23/4/04
            // was         if (fabs(sv[i]) < maxsv*seuilvp)
            if (fabs(sv[i]) <= maxsv*svThreshold)
            {
              cons[i][j] = v[j][i];
            }
        }

        vpMatrix Ker(ddl-rank,ddl) ;
        unsigned int k =0 ;
        for (j = 0 ; j < ddl ; j++)
        {
          //    cout << cons.Row(j+1) << " = " << cons.Row(j+1).SumSquare() << endl ;

          //if (cons.row(j+1).sumSquare() !=0)
          if (std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
          {
            for (i=0 ; i < cons.getCols() ; i++)
              Ker[k][i] = cons[j][i] ;
            //  Ker.Row(k+1) = cons.Row(j+1) ;
            k++;
          }
        }
        kerA = Ker ;
      }
    }
#if (DEBUG_LEVEL1) 
    std::cout << "end Kernel" << std::endl ;
#endif
    return rank ;
}

double vpMatrix::det(vpDetMethod method) const
{
  double det_ = 0;

  if ( method == LU_DECOMPOSITION )
  {
    det_ = this->detByLU();
  }

  return (det_);
}


bool
vpMatrix::saveMatrix(const char *filename, const vpMatrix &M, 
                     const bool binary, const char *header)
{
  std::fstream file;

  if (!binary)
    file.open(filename, std::fstream::out);
  else
    file.open(filename, std::fstream::out|std::fstream::binary);

  if(!file)
  {
    file.close();
    return false;
  }

  else
  {
    if (!binary)
    {
      unsigned int i = 0;
      file << "# ";
      while (header[i] != '\0')
      {
        file << header[i];
        if (header[i] == '\n')
          file << "# ";
        i++;
      }
      file << '\0';
      file << std::endl;
      file << M.getRows() << "\t" << M.getCols() << std::endl;
      file << M << std::endl;
    }
    else
    {
      int headerSize = 0;
      while (header[headerSize] != '\0') headerSize++;
      file.write(header,headerSize+1);
      unsigned int matrixSize;
      matrixSize = M.getRows();
      file.write((char*)&matrixSize,sizeof(int));
      matrixSize = M.getCols();
      file.write((char*)&matrixSize,sizeof(int));
      double value;
      for(unsigned int i = 0; i < M.getRows(); i++)
      {
        for(unsigned int j = 0; j < M.getCols(); j++)
        {
          value = M[i][j];
          file.write((char*)&value,sizeof(double));
        }
      }
    }
  }

  file.close();
  return true;
}

bool vpMatrix::saveMatrixYAML(const char *filename, const vpMatrix &M, const char *header)
{
    std::fstream file;

    file.open(filename, std::fstream::out);

    if(!file)
    {
        file.close();
        return false;
    }

    unsigned int i = 0;
    bool inIndent = false;
    std::string indent = "";
    bool checkIndent = true;
    while (header[i] != '\0')
    {
        file << header[i];
        if(checkIndent)
        {
            if (inIndent)
            {
                if(header[i] == ' ')
                    indent +=  " ";
                else if (indent.length() > 0)
                    checkIndent = false;
            }
            if (header[i] == '\n' || (inIndent && header[i] == ' '))
                inIndent = true;
            else
                inIndent = false;
        }
        i++;
    }

    if(i != 0)
        file << std::endl;
    file << "rows: " << M.getRows() << std::endl;
    file << "cols: " << M.getCols() << std::endl;

    if(indent.length() == 0)
        indent = "  ";

    file << "data: " << std::endl;
    unsigned int j;
    for(i=0;i<M.getRows();++i)
    {
        file << indent << "- [";
        for(j=0;j<M.getCols()-1;++j)
            file << M[i][j] << ", ";
        file << M[i][j] << "]" << std::endl;
    }

    file.close();
    return true;
}


bool
vpMatrix::loadMatrix(const char *filename, vpMatrix &M, const bool binary, 
                     char *header)
{
  std::fstream file;

  if (!binary)
    file.open(filename, std::fstream::in);
  else
    file.open(filename, std::fstream::in|std::fstream::binary);

  if(!file)
  {
    file.close();
    return false;
  }

  else
  {
    if (!binary)
    {
      char c='0';
      std::string h;
      while ((c != '\0') && (c != '\n'))
      {
        file.read(&c,1);
        h+=c;
      }
      if (header != NULL)
        strncpy(header, h.c_str(), h.size() + 1);

      unsigned int rows, cols;
      file >> rows;
      file >> cols;

      if (rows > std::numeric_limits<unsigned int>::max()
          || cols > std::numeric_limits<unsigned int>::max())
        throw vpException(vpException::badValue, "Matrix exceed the max size.");

      M.resize(rows,cols);

      double value;
      for(unsigned int i = 0; i < rows; i++)
      {
        for(unsigned int j = 0; j < cols; j++)
        {
          file >> value;
          M[i][j] = value;
        }
      }
    }
    else
    {
      char c='0';
      std::string h;
      while ((c != '\0') && (c != '\n'))
      {
        file.read(&c,1);
        h+=c;
      }
      if (header != NULL)
        strncpy(header, h.c_str(), h.size() + 1);

      unsigned int rows, cols;
      file.read((char*)&rows,sizeof(unsigned int));
      file.read((char*)&cols,sizeof(unsigned int));
      M.resize(rows,cols);

      double value;
      for(unsigned int i = 0; i < rows; i++)
      {
        for(unsigned int j = 0; j < cols; j++)
        {
          file.read((char*)&value,sizeof(double));
          M[i][j] = value;
        }
      }
    }
  }

  file.close();
  return true;
}


bool
vpMatrix::loadMatrixYAML(const char *filename, vpMatrix &M, char *header)
{
    std::fstream file;

    file.open(filename, std::fstream::in);

    if(!file)
    {
        file.close();
        return false;
    }

    unsigned int rows = 0,cols = 0;
    std::string h;
    std::string line,subs;
    bool inheader = true;
    unsigned int i=0, j;
    unsigned int lineStart = 0;

    while ( getline (file,line) )
    {
        if(inheader)
        {
            if(rows == 0 && line.compare(0,5,"rows:") == 0)
            {
                std::stringstream ss(line);
                ss >> subs;
                ss >> rows;
            }
            else if (cols == 0 && line.compare(0,5,"cols:") == 0)
            {
                std::stringstream ss(line);
                ss >> subs;
                ss >> cols;
            }
            else if (line.compare(0,5,"data:") == 0)
                inheader = false;
            else
                h += line + "\n";
        }
        else
        {
            // if i == 0, we just got out of the header: initialize matrix dimensions
            if(i == 0)
            {
                if(rows == 0 || cols == 0)
                {
                    file.close();
                    return false;
                }
                M.resize(rows, cols);
                // get indentation level which is common to all lines
                lineStart = (unsigned int)line.find("[") + 1;
            }
            std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
            j = 0;
            while(getline(ss, subs, ','))
                M[i][j++] = atof(subs.c_str());
            i++;
        }
    }

    if (header != NULL)
      strncpy(header, h.substr(0,h.length()-1).c_str(), h.size());

    file.close();
    return true;
}

vpMatrix
vpMatrix::expm()
{
  if(colNum != rowNum)
  {
    vpTRACE("The matrix must be square");
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
      "The matrix must be square" ));
  }
  else
  {
#ifdef VISP_HAVE_GSL
    size_t size_ = rowNum * colNum;
    double *b = new double [size_];
    for (size_t i=0; i< size_; i++)
      b[i] = 0.;
    gsl_matrix_view m  = gsl_matrix_view_array(this->data, rowNum, colNum);
    gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
    gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
    //gsl_matrix_fprintf(stdout, &em.matrix, "%g");
    vpMatrix expA(rowNum, colNum);
    memcpy(expA.data, b, size_ * sizeof(double));

    delete [] b;
    return expA;
#else
    vpMatrix _expE(rowNum, colNum);
    vpMatrix _expD(rowNum, colNum);
    vpMatrix _expX(rowNum, colNum);
    vpMatrix _expcX(rowNum, colNum);
    vpMatrix _eye(rowNum, colNum);

    _eye.setIdentity();
    vpMatrix exp(*this);

    //      double f;
    int e;
    double c = 0.5;
    int q = 6;
    int p = 1;

    double nA = 0;
    for (unsigned int i = 0; i < rowNum;i++)
    {
      double sum = 0;
      for (unsigned int j=0; j < colNum; j++)
      {
        sum += fabs((*this)[i][j]);
      }
      if (sum>nA||i==0)
      {
        nA=sum;
      }
    }

    /* f = */ frexp(nA, &e);
    //double s = (0 > e+1)?0:e+1;
    double s = e+1;

    double sca = 1.0 / pow(2.0,s);
    exp=sca*exp;
    _expX=*this;
    _expE=c*exp+_eye;
    _expD=-c*exp+_eye;
    for (int k=2;k<=q;k++)
    {
      c = c * ((double)(q-k+1)) / ((double)(k*(2*q-k+1)));
      _expcX=exp*_expX;
      _expX=_expcX;
      _expcX=c*_expX;
      _expE=_expE+_expcX;
      if (p) _expD=_expD+_expcX;
      else _expD=_expD- _expcX;
      p = !p;
    }
    _expX=_expD.inverseByLU();
    exp=_expX*_expE;
    for (int k=1;k<=s;k++)
    {
      _expE=exp*exp;
      exp=_expE;
    }
    return exp;
#endif
  }
}

double
vpMatrix::getMinValue() const
{
  double *dataptr = data;
  double min = *dataptr;
  dataptr++;
  for (unsigned int i = 0; i < dsize-1; i++)
  {
    if (*dataptr < min) min = *dataptr;
    dataptr++;
  }
  return min;
}

double
vpMatrix::getMaxValue() const
{
  double *dataptr = data;
  double max = *dataptr;
  dataptr++;
  for (unsigned int i = 0; i < dsize-1; i++)
  {
    if (*dataptr > max) max = *dataptr;
    dataptr++;
  }
  return max;
}


/**************************************************************************************************************/
/**************************************************************************************************************/


//Specific functions

/*
input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.

output:: the complement matrix of the element (rowNo,colNo).
This is the matrix obtained from M after elimenating the row rowNo and column colNo 

example:
1 2 3
M = 4 5 6
7 8 9
1 3
subblock(M, 1, 1) give the matrix 7 9
*/
vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
{
  vpMatrix M_comp(M.getRows()-1,M.getCols()-1);

  for ( unsigned int i = 0 ; i < col ; i++)
  {
    for ( unsigned int j = 0 ; j < row ; j++)
      M_comp[i][j]=M[i][j];
    for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
      M_comp[i][j-1]=M[i][j];
  }
  for ( unsigned int i = col+1 ; i < M.getCols(); i++)
  {
    for ( unsigned int j = 0 ; j < row ; j++)
      M_comp[i-1][j]=M[i][j];
    for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
      M_comp[i-1][j-1]=M[i][j];
  }
  return M_comp;
}

double vpMatrix::cond()
{
  vpMatrix v;
  vpColVector w;

  vpMatrix M;
  M = *this;

  M.svd(w, v);
  double min=w[0];
  double max=w[0];
  for(unsigned int i=0;i<M.getCols();i++)
  {
    if(min>w[i])min=w[i];
    if(max<w[i])max=w[i];
  }
  return max/min;
}

void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
{
  if(H.getCols() != H.getRows())
  {
    vpTRACE("The matrix must be square");
    throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
                            "The matrix must be square" ));
  }
  HLM.resize(H.getRows(), H.getCols());

  for(unsigned int i=0;i<H.getCols();i++)
  {
    for(unsigned int j=0;j<H.getCols();j++)
    {
      HLM[i][j]=H[i][j];
      if(i==j)
        HLM[i][j]+= alpha*H[i][j];
    }
  }

}

#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2

/*
* Local variables:
* c-basic-offset: 2
* End:
*/