vpMatrix.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2015 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://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * 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
 *
 *****************************************************************************/



#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

#include <visp3/core/vpConfig.h>

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

#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpTranslationVector.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpDebug.h>

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


vpMatrix::vpMatrix(const vpMatrix &M,
                   unsigned int r, unsigned int c, 
                   unsigned int nrows, unsigned int ncols)
  : vpArray2D<double>()
{
  if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
    throw(vpException(vpException::dimensionError,
                      "Cannot construct a sub matrix (%dx%d) starting at position (%d,%d) that is not contained in the original matrix (%dx%d)",
                      nrows, ncols, r, c, M.rowNum, M.colNum)) ;
  }

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

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::eye(unsigned int n)
{
  try {
    eye(n, n);
  }
  catch(...) {
    throw ;
  }
}

void
vpMatrix::eye(unsigned int m, unsigned int n)
{
  try {
    resize(m,n) ;
  }
  catch(...) {
    throw ;
  }

  eye();
}

void
vpMatrix::eye()
{
  for (unsigned int i=0; i<rowNum; i++) {
    for (unsigned int j=0; j<colNum; j++) {
      if (i == j) (*this)[i][j] = 1.0;
      else        (*this)[i][j] = 0;
    }
  }
}

void
vpMatrix::setIdentity(const double & val)
{
  for (unsigned int i=0;i<rowNum;i++)
    for (unsigned int j=0;j<colNum;j++)
      if (i==j) (*this)[i][j] = val ;
      else      (*this)[i][j] = 0;
}


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

  try {
    At.resize(colNum, rowNum);
  }
  catch(...)
  {
    throw ;
  }

  for (unsigned int i=0;i<rowNum;i++) {
    double *coli = (*this)[i] ;
    for (unsigned int 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(...)
  {
    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(...)
  {
    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(...)
  {
    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;
}

vpMatrix &
vpMatrix::operator=(const vpArray2D<double> &A)
{
  try {
    resize(A.getRows(), A.getCols()) ;
  }
  catch(...) {
    throw ;
  }

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

  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;
}


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;
}

void
vpMatrix::diag(const vpColVector &A)
{
  unsigned int rows = A.getRows() ;
  try {
    this->resize(rows,rows) ;
  }
  catch(...) {
    throw ;
  }
  (*this) = 0 ;
  for (unsigned int i=0 ; i< rows ; i++ )
    (* this)[i][i] = A[i] ;
}

void
vpMatrix::diag(const double &val)
{
  (*this) = 0;
  unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
  for (unsigned int i=0 ; i< min_ ; i++ )
    (* this)[i][i] = val;
}


void
vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
{
  unsigned int rows = A.getRows() ;
  try {
    DA.resize(rows,rows) ;
  }
  catch(...)
  {
    throw ;
  }
  DA =0 ;
  for (unsigned int i=0 ; i< rows ; i++ )
    DA[i][i] = A[i] ;
}

vpTranslationVector
vpMatrix::operator*(const vpTranslationVector &tv) const
{
  vpTranslationVector t_out;

  if (rowNum != 3 || colNum != 3) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (%dx%d) matrix by a (%dx%d) translation vector",
                      rowNum, colNum, tv.getRows(), tv.getCols())) ;
  }

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

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

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

void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
{
  if (A.colNum != v.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
                      A.getRows(), A.getCols(), v.getRows())) ;
  }

  try {
    if (A.rowNum != w.rowNum) w.resize(A.rowNum);
  }
  catch(...) {
    throw ;
  }

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

//---------------------------------
// 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(...) {
    throw ;
  }

  if (A.colNum != B.rowNum) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  // 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;
    }
  }
}

void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMatrix &C)
{
  if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  // 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;
    }
  }
}

void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneousMatrix &C)
{
  if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  // 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;
    }
  }
}

void vpMatrix::mult2Matrices(const vpMatrix &A, const vpColVector &B, vpColVector &C)
{
  vpMatrix::multMatrixVector(A, B, C);
}

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

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

  return C;
}

vpMatrix vpMatrix::operator*(const vpRotationMatrix &R) const
{
  if (colNum != R.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix",
                      rowNum, colNum)) ;
  }
  vpMatrix C(rowNum, 3);

  unsigned int RcolNum = R.getCols();
  unsigned int RrowNum = R.getRows();
  for (unsigned int i=0;i<rowNum;i++)
  {
    double *rowptri = rowPtrs[i];
    double *ci = C[i];
    for (unsigned int j=0;j<RcolNum;j++)
    {
      double s = 0;
      for (unsigned int k=0;k<RrowNum;k++) s += rowptri[k] * R[k][j];
      ci[j] = s;
    }
  }

  return C;
}
vpMatrix vpMatrix::operator*(const vpVelocityTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (3x3) velocity twist matrix",
                      rowNum, colNum)) ;
  }
  vpMatrix M(rowNum, 6);

  unsigned int VcolNum = V.getCols();
  unsigned int VrowNum = V.getRows();
  for (unsigned int i=0;i<rowNum;i++)
  {
    double *rowptri = rowPtrs[i];
    double *ci = M[i];
    for (unsigned int j=0;j<VcolNum;j++)
    {
      double s = 0;
      for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
      ci[j] = s;
    }
  }

  return M;
}
vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot multiply (%dx%d) matrix by (3x3) force/torque twist matrix",
                      rowNum, colNum)) ;
  }
  vpMatrix M(rowNum, 6);

  unsigned int VcolNum = V.getCols();
  unsigned int VrowNum = V.getRows();
  for (unsigned int i=0;i<rowNum;i++)
  {
    double *rowptri = rowPtrs[i];
    double *ci = M[i];
    for (unsigned int j=0;j<VcolNum;j++)
    {
      double s = 0;
      for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
      ci[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(...) {
    throw ;
  }

  if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (%dx%d) matrix with (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  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(...) {
    throw ;
  }

  if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (%dx%d) matrix with (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  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] = BrowPtrs[i][j]+ArowPtrs[i][j];
    }
  }
}

void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
  try  {
    if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum);
  }
  catch(...) {
    throw ;
  }

  if ((A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (%dx%d) matrix with (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  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] = BrowPtrs[i][j]+ArowPtrs[i][j];
    }
  }
}

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


void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
  try {
    if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum);
  }
  catch(...) {
    throw ;
  }

  if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  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] = ArowPtrs[i][j]-BrowPtrs[i][j];
    }
  }
}

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(...) {
    throw ;
  }

  if ( (A.colNum != B.getCols())||(A.rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
                      A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
  }

  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] = ArowPtrs[i][j]-BrowPtrs[i][j];
    }
  }
}

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())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot add (%dx%d) matrix to (%dx%d) matrix",
                      rowNum, colNum, B.getRows(), B.getCols())) ;
  }

  double ** BrowPtrs=B.rowPtrs;

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

  return *this;
}

vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
{
  if ( (colNum != B.getCols())||(rowNum != B.getRows())) {
    throw(vpException(vpException::dimensionError,
                      "Cannot substract (%dx%d) matrix to (%dx%d) matrix",
                      rowNum, colNum, B.getRows(), B.getCols())) ;
  }

  double ** BrowPtrs=B.rowPtrs;
  for (unsigned int i=0;i<rowNum;i++)
    for(unsigned int j=0;j<colNum;j++)
      rowPtrs[i][j] -= BrowPtrs[i][j];

  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(...) {
    throw ;
  }

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

  //    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];
}

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


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.
//---------------------------------




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

vpMatrix operator*(const double &x,const vpMatrix &B)
{
  vpMatrix C(B.getRows(), B.getCols());

  unsigned int Brow = B.getRows() ;
  unsigned int Bcol = B.getCols() ;

  for (unsigned int i=0;i<Brow;i++)
    for(unsigned int j=0;j<Bcol;j++)
      C[i][j]= B[i][j]*x;

  return C ;
}

vpMatrix vpMatrix::operator*(double x) const
{
  vpMatrix M(rowNum,colNum);

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

  return M;
}

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

  try {
    C.resize(rowNum,colNum);
  }
  catch(...) {
    throw ;
  }

  //if (x == 0) {
  if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
    throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
  }

  double  xinv = 1/x ;

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

  return C;
}


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;
}


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;
}

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;
}

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

  double xinv = 1/x ;

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

  return *this;
}

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







void vpMatrix::stackColumns(vpColVector  &out ){

  try {
    if ((out.rowNum != colNum*rowNum) || (out.colNum != 1)) out.resize(rowNum);
  }
  catch(...) {
    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.getRows() != 1) || (out.getCols() != colNum*rowNum)) out.resize(rowNum);
  }
  catch(...) {
    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(vpException(vpException::dimensionError,
                      "In Kronecker product bad dimension of output matrix"));
  }

  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) const
{
  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) const
{
  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 1 /* no verification */
  {
    w.resize( this->getCols() );
    v.resize( this->getCols(), this->getCols() );

#if defined (VISP_HAVE_LAPACK_C)
    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 0 // debug
  {
    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 0 // debug
  {
    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 ;
}

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::stack(const vpMatrix &A, const vpMatrix &B)
{
  vpMatrix C ;

  try{
    vpMatrix::stack(A, B, C) ;
  }
  catch(...) {
    throw ;
  }

  return C ;
}

vpMatrix
vpMatrix::stack(const vpMatrix &A, const vpRowVector &r)
{
  vpMatrix C ;

  try{
    vpMatrix::stack(A, r, C) ;
  }
  catch(...) {
    throw ;
  }

  return C ;
}

void
vpMatrix::stack(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()) {
      throw(vpException(vpException::dimensionError,
                        "Cannot stack (%dx%d) matrix with (%dx%d) matrix",
                        A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
    }

  try {
    C.resize(nra+nrb,B.getCols()  ) ;
  }
  catch(...) {
    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] ;
    }
  }
}

void
vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
{
  unsigned int nra = A.getRows() ;

  if (nra !=0)
    if (A.getCols() != r.getCols()) {
      throw(vpException(vpException::dimensionError,
                        "Cannot stack (%dx%d) matrix with (1x%d) row vector",
                        A.getRows(), A.getCols(), r.getCols())) ;
    }

  try {
    C.resize(nra+1,r.getCols()  ) ;
  }
  catch(...) {
    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 (j=0 ; j < r.getCols() ; j++) {
    C[nra][j] = r[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(...) {
    throw;
  }

  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(...)  {
        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 vpException(vpException::dimensionError,
                      "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
                      B.getRows(), B.getCols(), A.getCols(), A.getRows(), r, c);
  }
}

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

  try{
    juxtaposeMatrices(A,B, C) ;
  }
  catch(...) {
    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()) {
      throw(vpException(vpException::dimensionError,
                        "Cannot juxtapose (%dx%d) matrix with (%dx%d) matrix",
                        A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
    }

    try {
      C.resize(B.getRows(),nca+ncb) ;
    }
    catch(...) {
      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] ;
      }
}


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

int
vpMatrix::print(std::ostream& s, unsigned int length, char const* intro) const
{
  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
{
  os << "[ ";
  for (unsigned int i=0; i < this->getRows(); ++ i) {
    for (unsigned int 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
{
  os << "([ " << std::endl;
  for (unsigned int i=0; i < this->getRows(); ++ i) {
    os << "[";
    for (unsigned int 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
{
  for (unsigned int i=0; i < this->getRows(); ++ i) {
    for (unsigned int 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
{
  const char defaultName [] = "A";
  if (NULL == matrixName)
  {
    matrixName = defaultName;
  }
  os << "vpMatrix " << defaultName
    << " (" << this ->getRows ()
    << ", " << this ->getCols () << "); " <<std::endl;

  for (unsigned int i=0; i < this->getRows(); ++ i)
  {
    for (unsigned int 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::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 {
    throw(vpException(vpException::fatalError,
                      "Cannot compute LU decomposition on a non square matrix (%dx%d)",
                      rowNum, colNum)) ;
  }
  return det_ ;
}



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

void vpMatrix::stack(const vpRowVector &r)
{
  if(rowNum == 0)
    *this = r;
  else
    *this = vpMatrix::stack(*this, r);
}


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 vpException(vpException::dimensionError,
                      "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
                      A.getRows(), A.getCols(), rowNum, colNum, r, c);
  }
}


vpColVector vpMatrix::eigenValues() const
{
  if (rowNum != colNum) {
    throw(vpException(vpException::dimensionError,
                      "Cannot compute eigen values on a non square matrix (%dx%d)",
                      rowNum, colNum)) ;
  }

#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()) {
          throw(vpException(vpException::fatalError,
                            "Cannot compute eigen values on a non symetric matrix")) ;
        }
      }
    }

    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
  {
    throw(vpException(vpException::functionNotImplementedError,
                      "Eigen values computation is not implemented. You should install GSL rd party")) ;
  }
#endif  
}

#ifdef VISP_HAVE_GSL  /* be careful of the copy below */
void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
#else
void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
#endif
{
  if (rowNum != colNum) {
    throw(vpException(vpException::dimensionError,
                      "Cannot compute eigen values on a non square matrix (%dx%d)",
                      rowNum, colNum)) ;
  }

#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()) {
          throw(vpException(vpException::fatalError,
                            "Cannot compute eigen values on a non symetric matrix")) ;
        }
      }
    }

    // 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];
      }
    }

    gsl_eigen_symmv_free (w);
    gsl_vector_free (eval);
    gsl_matrix_free (m);
    gsl_matrix_free (evec);
  }
#else
  {
    throw(vpException(vpException::functionNotImplementedError,
                      "Eigen values computation is not implemented. You should install GSL rd party")) ;
  }
#endif  
}


unsigned int 
vpMatrix::kernel(vpMatrix &kerA, double svThreshold) const
{
  unsigned int i, j ;
  unsigned int nbline = getRows() ;
  unsigned int nbcol = getCols() ;

  if ( (nbline <= 0) || (nbcol <= 0) ) {
    throw( vpException(vpException::dimensionError, "Cannot compute kernel of a zero-size matrix") );
  }

  vpMatrix A ; // Copy of the matrix, SVD function is destructive
  vpColVector sv(nbcol) ;   // singular values
  vpMatrix v(nbcol,nbcol) ; // V matrix of singular value decomposition

  // Copy and resize matrix to have at least as many rows as columns
  // kernel is computed in svd method only if the matrix has more rows than columns

  if (nbline < nbcol) A.resize(nbcol,nbcol) ;
  else A.resize(nbline,nbcol) ;

  for (i=0 ; i < nbline ; i++)
  {
    for (j=0 ; j < nbcol ; j++)
    {
      A[i][j] = (*this)[i][j] ;
    }
  }

  A.svd(sv,v);

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

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

  if (rank != nbcol)
  {
    vpMatrix Ker(nbcol-rank,nbcol) ;
    unsigned int k = 0 ;
    for (j = 0 ; j < nbcol ; j++)
    {
      //if( v.col(j) in kernel and non zero )
      if ( (fabs(sv[j]) <= maxsv*svThreshold) && (std::fabs(v.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon()))
      {
        //  Ker.Row(k) = v.Col(j) ;
        for (i=0 ; i < v.getRows() ; i++)
        {
          Ker[k][i] = v[i][j];
        }
        k++;
      }
    }
    kerA = Ker ;
  }
  else
  {
    kerA.resize(0,0);
  }

  return rank ;
}

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

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

  return (det_);
}

vpMatrix
vpMatrix::expm() const
{
  if(colNum != rowNum) {
    throw(vpException(vpException::dimensionError,
                      "Cannot compute the exponential of a non square (%dx%d) matrix",
                      rowNum, colNum ));
  }
  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.eye();
    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
  }
}

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


//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() const
{
  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()) {
    throw(vpException(vpException::dimensionError,
                      "Cannot compute HLM on a non square matrix (%dx%d)",
                      H.getRows(), H.getCols() ));
  }
  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];
    }
  }
}

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::sumSquare() const
{
  double sum_square=0.0;
  double x ;

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

  return sum_square;
}

#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
vpMatrix vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B)
{
  return (vpMatrix)(vpColVector::stack(A, B));
}

void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
  vpColVector::stack(A, B, C);
}

vpMatrix vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B)
{
  return vpMatrix::stack(A, B);
};

void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C)
{
  vpMatrix::stack(A, B, C);
};
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)