vpMatrix.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 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 as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * 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 <algorithm>
#include <assert.h>
#include <cmath> // std::fabs
#include <fstream>
#include <limits> // numeric_limits
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <vector>

#include <visp3/core/vpConfig.h>

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

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

#define USE_SSE_CODE 1
#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
#include <emmintrin.h>
#define VISP_HAVE_SSE2 1
#endif

#if VISP_HAVE_SSE2 && USE_SSE_CODE
#define USE_SSE 1
#endif

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

void compute_pseudo_inverse(const vpMatrix &a, const vpColVector &sv, const vpMatrix &v, unsigned int nrows,
                            unsigned int ncols, unsigned int nrows_orig, unsigned int ncols_orig, double svThreshold,
                            vpMatrix &Ap, unsigned int &rank)
{
  vpMatrix a1(ncols, nrows);

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

  rank = 0;

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

    for (unsigned int j = 0; j < nrows; j++) {
      a1[i][j] = 0.0;

      for (unsigned int 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();
}

void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows_orig,
                            unsigned int ncols_orig, double svThreshold, vpMatrix &Ap, unsigned int &rank,
                            vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt)
{
  Ap.resize(ncols_orig, nrows_orig);

  // compute the highest singular value and the rank of h
  double maxsv = fabs(sv[0]);

  rank = 0;

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

    for (unsigned int j = 0; j < nrows_orig; j++) {
      //      Ap[i][j] = 0.0;

      for (unsigned int k = 0; k < ncols_orig; k++) {
        if (fabs(sv[k]) > maxsv * svThreshold) {
          Ap[i][j] += V[i][k] * U[j][k] / sv[k];
        }
      }
    }
  }

  // Compute im(A) and im(At)
  imA.resize(nrows_orig, rank);
  imAt.resize(ncols_orig, rank);

  for (unsigned int i = 0; i < nrows_orig; i++) {
    for (unsigned int j = 0; j < rank; j++) {
      imA[i][j] = U[i][j];
    }
  }

  for (unsigned int i = 0; i < ncols_orig; i++) {
    for (unsigned int j = 0; j < rank; j++) {
      imAt[i][j] = V[i][j];
    }
  }

  kerAt.resize(ncols_orig - rank, ncols_orig);
  if (rank != ncols_orig) {
    for (unsigned int j = 0, k = 0; j < ncols_orig; 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())) {
        for (unsigned int i = 0; i < V.getRows(); i++) {
          kerAt[k][i] = V[i][j];
        }
        k++;
      }
    }
  }
}

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

#ifdef VISP_HAVE_CPP11_COMPATIBILITY
vpMatrix::vpMatrix(vpMatrix &&A) : vpArray2D<double>()
{
  rowNum = A.rowNum;
  colNum = A.colNum;
  rowPtrs = A.rowPtrs;
  dsize = A.dsize;
  data = A.data;

  A.rowNum = 0;
  A.colNum = 0;
  A.rowPtrs = NULL;
  A.dsize = 0;
  A.data = NULL;
}
#endif

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, false, false);

  if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
    return;                  // Noting to do
  for (unsigned int i = 0; i < nrows; i++) {
    memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
  }
}

vpMatrix vpMatrix::extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
{
  unsigned int rnrows = r + nrows;
  unsigned int cncols = c + ncols;

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

  vpMatrix M(nrows, ncols);
  for (unsigned int i = 0; i < nrows; i++) {
    memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
  }

  return M;
}

void vpMatrix::eye(unsigned int n) { eye(n, n); }

void vpMatrix::eye(unsigned int m, unsigned int n)
{
  resize(m, n);

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

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

  At.resize(colNum, rowNum, false, false);

  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
{
  At.resize(colNum, rowNum, false, false);

  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
{
  if ((B.rowNum != rowNum) || (B.colNum != rowNum))
    B.resize(rowNum, rowNum, false, false);

  // 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
{
  if ((B.rowNum != colNum) || (B.colNum != colNum))
    B.resize(colNum, colNum, false, false);

#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
  double alpha = 1.0;
  double beta = 0.0;
  char transa = 'n';
  char transb = 't';

  vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data, colNum);
#else
  unsigned int i, j, k;
  double s;
  double *ptr;
  for (i = 0; i < colNum; i++) {
    double *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;
  }
#endif
}

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

  AtA(B);

  return B;
}

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

  if (data != NULL && A.data != NULL && data != A.data) {
    memcpy(data, A.data, dsize * sizeof(double));
  }

  return *this;
}

#ifdef VISP_HAVE_CPP11_COMPATIBILITY
vpMatrix &vpMatrix::operator=(const vpMatrix &A)
{
  resize(A.getRows(), A.getCols(), false, false);

  if (data != NULL && A.data != NULL && data != A.data) {
    memcpy(data, A.data, dsize * sizeof(double));
  }

  return *this;
}

vpMatrix &vpMatrix::operator=(vpMatrix &&other)
{
  if (this != &other) {
    free(data);
    free(rowPtrs);

    rowNum = other.rowNum;
    colNum = other.colNum;
    rowPtrs = other.rowPtrs;
    dsize = other.dsize;
    data = other.data;

    other.rowNum = 0;
    other.colNum = 0;
    other.rowPtrs = NULL;
    other.dsize = 0;
    other.data = NULL;
  }

  return *this;
}
#endif

vpMatrix &vpMatrix::operator=(double x)
{
  std::fill(data, data + rowNum*colNum, 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();
  this->resize(rows, rows);

  (*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();
  DA.resize(rows, rows);

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

  if (A.rowNum != w.rowNum)
    w.resize(A.rowNum, false);

#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
  double alpha = 1.0;
  double beta = 0.0;
  char trans = 't';
  int incr = 1;

  vpMatrix::blas_dgemv(trans, A.colNum, A.rowNum, alpha, A.data, A.colNum, v.data, incr, beta, w.data, incr);
#else
  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;
    }
  }
#endif
}

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

void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
    C.resize(A.rowNum, B.colNum, false, false);

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

#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
  double alpha = 1.0;
  double beta = 0.0;
  char trans = 'n';

  vpMatrix::blas_dgemm(trans, trans, B.colNum, A.rowNum, A.colNum, alpha, B.data, B.colNum, A.data, A.colNum, beta,
                       C.data, B.colNum);
#else
  // 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;
    }
  }
#endif
}

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 (6x6) velocity twist matrix",
                      rowNum, colNum));
  }
  vpMatrix M;
  M.resize(rowNum, 6, false, false);

#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
  double alpha = 1.0;
  double beta = 0.0;
  char trans = 'n';

  vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
                       V.colNum);
#else
  bool checkSSE2 = vpCPUFeatures::checkSSE2();
#if !USE_SSE
  checkSSE2 = false;
#endif

  if (checkSSE2) {
#if USE_SSE
    vpMatrix V_trans(6, 6);
    for (unsigned int i = 0; i < 6; i++) {
      for (unsigned int j = 0; j < 6; j++) {
        V_trans[i][j] = V[j][i];
      }
    }

    for (unsigned int i = 0; i < rowNum; i++) {
      double *rowptri = rowPtrs[i];
      double *ci = M[i];

      for (int j = 0; j < 6; j++) {
        __m128d v_mul = _mm_setzero_pd();
        for (int k = 0; k < 6; k += 2) {
          v_mul = _mm_add_pd(v_mul, _mm_mul_pd(_mm_loadu_pd(&rowptri[k]), _mm_loadu_pd(&V_trans[j][k])));
        }

        double v_tmp[2];
        _mm_storeu_pd(v_tmp, v_mul);
        ci[j] = v_tmp[0] + v_tmp[1];
      }
    }
#endif
  } else {
    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;
      }
    }
  }
#endif

  return M;
}
vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) 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)
{
  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
    C.resize(A.rowNum, B.colNum, false, false);

  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)
{
  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
    C.resize(A.rowNum, B.colNum, false, false);

  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)
{
  if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum))
    C.resize(A.rowNum);

  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)
{
  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
    C.resize(A.rowNum);

  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)
{
  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
    C.resize(A.rowNum, A.colNum, false, false);

  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)
{
  if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
    C.resize(A.rowNum, A.colNum, false, false);

  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;

  C.resize(rowNum, colNum, false, false);

  // 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)
{
  if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
    out.resize(colNum * rowNum, false, false);

  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)
{
  if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
    out.resize(colNum * rowNum, false, false);

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

vpMatrix vpMatrix::hadamard(const vpMatrix &m) const
{
  if (m.getRows() != rowNum || m.getCols() != colNum) {
    throw(vpException(vpException::dimensionError, "In Hadamard product: bad dimension of input matrix"));
  }

  vpMatrix out;
  out.resize(rowNum, colNum, false);

  unsigned int i = 0;

#if VISP_HAVE_SSE2
  if (vpCPUFeatures::checkSSE2() && dsize >= 2) {
    for (; i <= dsize - 2; i += 2) {
      __m128d vout = _mm_mul_pd(_mm_loadu_pd(data + i), _mm_loadu_pd(m.data + i));
      _mm_storeu_pd(out.data + i, vout);
    }
  }
#endif

  for (; i < dsize; i++) {
    out.data[i] = data[i] * m.data[i];
  }

  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 defined(VISP_HAVE_LAPACK)
  svdLapack(w, V);
#elif defined(VISP_HAVE_EIGEN3)
  svdEigen3(w, V);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
  svdOpenCV(w, V);
#elif defined(VISP_HAVE_GSL)
  svdGsl(w, V);
#else
  (void)w;
  (void)V;
  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party"));
#endif
}

unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, svThreshold);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, svThreshold);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, svThreshold);
#elif defined(VISP_HAVE_GSL)
  return pseudoInverseGsl(Ap, svThreshold);
#else
  (void)Ap;
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                                             "Install Lapack, Eigen3, OpenCV "
                                             "or GSL 3rd party"));
#endif
}

vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(svThreshold);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(svThreshold);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(svThreshold);
#elif defined(VISP_HAVE_GSL)
  return pseudoInverseGsl(svThreshold);
#else
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                                             "Install Lapack, Eigen3, OpenCV "
                                             "or GSL 3rd party"));
#endif
}

#ifndef DOXYGEN_SHOULD_SKIP_THIS
#if defined(VISP_HAVE_LAPACK)

vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();

  vpMatrix Ap(ncols_orig, nrows_orig);

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

  vpMatrix U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdLapack(sv, V);

  unsigned int rank;
  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return Ap;
}

unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdLapack(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}
unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  sv.resize(ncols);

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

  U.svdLapack(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}

unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerA) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  unsigned int rank;
  vpMatrix U, V;
  vpColVector sv_;

  if (nrows < ncols) {
    U.resize(ncols, ncols);
    sv.resize(nrows);
  } else {
    U.resize(nrows, ncols);
    sv.resize(ncols);
  }

  U.insert(*this, 0, 0);
  U.svdLapack(sv_, V);

  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);

  // Remove singular values equal to to that correspond to the lines of 0
  // introduced when m < n
  for (unsigned int i = 0; i < sv.size(); i++)
    sv[i] = sv_[i];

  return rank;
}
#endif
#if defined(VISP_HAVE_EIGEN3)

vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();

  vpMatrix Ap(ncols_orig, nrows_orig);

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

  vpMatrix U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdEigen3(sv, V);

  unsigned int rank;
  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return Ap;
}

unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdEigen3(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}
unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  sv.resize(ncols);

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

  U.svdEigen3(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}

unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerA) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  unsigned int rank;
  vpMatrix U, V;
  vpColVector sv_;

  if (nrows < ncols) {
    U.resize(ncols, ncols);
    sv.resize(nrows);
  } else {
    U.resize(nrows, ncols);
    sv.resize(ncols);
  }

  U.insert(*this, 0, 0);
  U.svdEigen3(sv_, V);

  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);

  // Remove singular values equal to to that correspond to the lines of 0
  // introduced when m < n
  for (unsigned int i = 0; i < sv.size(); i++)
    sv[i] = sv_[i];

  return rank;
}
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)

vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();

  vpMatrix Ap(ncols_orig, nrows_orig);

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

  vpMatrix U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdOpenCV(sv, V);

  unsigned int rank;
  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return Ap;
}

unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdOpenCV(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}
unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  sv.resize(ncols);

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

  U.svdOpenCV(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}

unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerA) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  unsigned int rank;
  vpMatrix U, V;
  vpColVector sv_;

  if (nrows < ncols) {
    U.resize(ncols, ncols);
    sv.resize(nrows);
  } else {
    U.resize(nrows, ncols);
    sv.resize(ncols);
  }

  U.insert(*this, 0, 0);
  U.svdOpenCV(sv_, V);

  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);

  // Remove singular values equal to to that correspond to the lines of 0
  // introduced when m < n
  for (unsigned int i = 0; i < sv.size(); i++)
    sv[i] = sv_[i];

  return rank;
}
#endif
#if defined(VISP_HAVE_GSL)

vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();

  vpMatrix Ap(ncols_orig, nrows_orig);

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

  vpMatrix U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdGsl(sv, V);

  unsigned int rank;
  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return Ap;
}

unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  vpColVector sv(ncols);

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

  U.svdGsl(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}
unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();
  unsigned int rank;

  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 U(nrows, ncols);
  vpMatrix V(ncols, ncols);
  sv.resize(ncols);

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

  U.svdGsl(sv, V);

  compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);

  return rank;
}

unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                        vpMatrix &imAt, vpMatrix &kerA) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  unsigned int rank;
  vpMatrix U, V;
  vpColVector sv_;

  if (nrows < ncols) {
    U.resize(ncols, ncols);
    sv.resize(nrows);
  } else {
    U.resize(nrows, ncols);
    sv.resize(ncols);
  }

  U.insert(*this, 0, 0);
  U.svdGsl(sv_, V);

  compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);

  // Remove singular values equal to to that correspond to the lines of 0
  // introduced when m < n
  for (unsigned int i = 0; i < sv.size(); i++)
    sv[i] = sv_[i];

  return rank;
}
#endif
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS

unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, svThreshold);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, svThreshold);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, svThreshold);
#elif defined(VISP_HAVE_GSL)
  return pseudoInverseGsl(Ap, sv, svThreshold);
#else
  (void)Ap;
  (void)sv;
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                                             "Install Lapack, Eigen3, OpenCV "
                                             "or GSL 3rd party"));
#endif
}

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

unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA, vpMatrix &imAt,
                                     vpMatrix &kerAt) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, svThreshold, imA, imAt, kerAt);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
#elif defined(VISP_HAVE_GSL)
  return pseudoInverseGsl(Ap, sv, svThreshold, imA, imAt, kerAt);
#else
  (void)Ap;
  (void)sv;
  (void)svThreshold;
  (void)imA;
  (void)imAt;
  (void)kerAt;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                                             "Install Lapack, Eigen3, OpenCV "
                                             "or GSL 3rd party"));
#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 column %u from the %ux%u matrix", j, getRows(), getCols()));
  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 column %u from the %ux%u matrix", j, getRows(), getCols()));
  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"));

  vpRowVector r;
  r.resize(colNum, false);

  if (r.data != NULL && data != NULL && r.data != data) {
    memcpy(r.data, data + i * colNum, sizeof(double) * colNum);
  }

  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;

  vpMatrix::stack(A, B, C);

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

  if (A.data != NULL && A.data == C.data) {
    std::cerr << "A and C must be two different objects!" << std::endl;
    return;
  }

  if (B.data != NULL && B.data == C.data) {
    std::cerr << "B and C must be two different objects!" << std::endl;
    return;
  }

  C.resize(nra + nrb, B.getCols(), false, false);

  if (C.data != NULL && A.data != NULL && A.size() > 0) {
    // Copy A in C
    memcpy(C.data, A.data, sizeof(double) * A.size());
  }

  if (C.data != NULL && B.data != NULL && B.size() > 0) {
    // Copy B in C
    memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
  }
}

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

  return C;
}

void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
{
  if (A.data != NULL && A.data == C.data) {
    std::cerr << "A and C must be two different objects!" << std::endl;
    return;
  }

  C = A;
  C.stack(r);
}

vpMatrix vpMatrix::stack(const vpMatrix &A, const vpColVector &c)
{
  vpMatrix C;
  vpMatrix::stack(A, c, C);

  return C;
}

void vpMatrix::stack(const vpMatrix &A, const vpColVector &c, vpMatrix &C)
{
  if (A.data != NULL && A.data == C.data) {
    std::cerr << "A and C must be two different objects!" << std::endl;
    return;
  }

  C = A;
  C.stack(c);
}

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

  insert(A, B, C, r, c);

  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())) {
    C.resize(A.getRows(), A.getCols(), false, false);

    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;

  juxtaposeMatrices(A, B, C);

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

  if (B.getRows() == 0 || nca + ncb == 0) {
    std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
    return;
  }

  C.resize(B.getRows(), nca + ncb, false, false);

  C.insert(A, 0, 0);
  C.insert(B, 0, nca);
}

//--------------------------------------------------------------------
// 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 std::string &matrixName, bool octet) const
{
  os << "vpMatrix " << matrixName << " (" << 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 << matrixName << "[" << i << "][" << j << "] = " << (*this)[i][j] << "; " << std::endl;
      } else {
        for (unsigned int k = 0; k < sizeof(double); ++k) {
          os << "((unsigned char*)&(" << matrixName << "[" << i << "][" << j << "]) )[" << k << "] = 0x" << std::hex
             << (unsigned int)((unsigned char *)&((*this)[i][j]))[k] << "; " << std::endl;
        }
      }
    }
    os << std::endl;
  }
  return os;
}

void vpMatrix::stack(const vpMatrix &A)
{
  if (rowNum == 0) {
    *this = A;
  } else if (A.getRows() > 0) {
    if (colNum != A.getCols()) {
      throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx%d) matrix", rowNum, colNum,
                        A.getRows(), A.getCols()));
    }

    unsigned int rowNumOld = rowNum;
    resize(rowNum + A.getRows(), colNum, false, false);
    insert(A, rowNumOld, 0);
  }
}

void vpMatrix::stack(const vpRowVector &r)
{
  if (rowNum == 0) {
    *this = r;
  } else {
    if (colNum != r.getCols()) {
      throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (1x%d) row vector", rowNum,
                        colNum, r.getCols()));
    }

    if (r.size() == 0) {
      return;
    }

    unsigned int oldSize = size();
    resize(rowNum + 1, colNum, false, false);

    if (data != NULL && r.data != NULL && data != r.data) {
      // Copy r in data
      memcpy(data + oldSize, r.data, sizeof(double) * r.size());
    }
  }
}

void vpMatrix::stack(const vpColVector &c)
{
  if (colNum == 0) {
    *this = c;
  } else {
    if (rowNum != c.getRows()) {
      throw(vpException(vpException::dimensionError, "Cannot stack (%dx%d) matrix with (%dx1) column vector", rowNum,
                        colNum, c.getRows()));
    }

    if (c.size() == 0) {
      return;
    }

    vpMatrix tmp = *this;
    unsigned int oldColNum = colNum;
    resize(rowNum, colNum + 1, false, false);

    if (data != NULL && tmp.data != NULL && data != tmp.data) {
      // Copy c in data
      for (unsigned int i = 0; i < rowNum; i++) {
        memcpy(data + i*colNum, tmp.data + i*oldColNum, sizeof(double) * oldColNum);
        rowPtrs[i][oldColNum] = c[i];
      }
    }
  }
}

void vpMatrix::insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
{
  if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
    if (A.colNum == colNum && data != NULL && A.data != NULL && A.data != data) {
      memcpy(data + r * colNum, A.data, sizeof(double) * A.size());
    } else if (data != NULL && A.data != NULL && A.data != data) {
      for (unsigned int i = r; i < (r + A.getRows()); i++) {
        memcpy(data + i * colNum + c, A.data + (i - r) * A.colNum, sizeof(double) * A.colNum);
      }
    }
  } 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 = (unsigned int)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 = (unsigned int)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 = (unsigned int)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 &kerAt, double svThreshold) const
{
  unsigned int nbline = getRows();
  unsigned int nbcol = getCols();

  vpMatrix U;               // 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)
    U.resize(nbcol, nbcol);
  else
    U.resize(nbline, nbcol);

  U.insert(*this, 0, 0);

  U.svd(sv, V);

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

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

  kerAt.resize(nbcol - rank, nbcol);
  if (rank != nbcol) {
    for (unsigned int j = 0, k = 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())) {
        for (unsigned int i = 0; i < V.getRows(); i++) {
          kerAt[k][i] = V[i][j];
        }
        k++;
      }
    }
  }

  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(), false, false);

  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;
  for (unsigned int i = 0; i < dsize; i++) {
    double x = *(data + i);
    norm += x * x;
  }

  return sqrt(norm);
}

double vpMatrix::infinityNorm() const
{
  double norm = 0.0;
  for (unsigned int i = 0; i < rowNum; i++) {
    double 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;
}

vpMatrix vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, const std::string &mode)
{
  vpMatrix res;
  conv2(M, kernel, res, mode);
  return res;
}

void vpMatrix::conv2(const vpMatrix &M, const vpMatrix &kernel, vpMatrix &res, const std::string &mode)
{
  if (M.getRows()*M.getCols() == 0 || kernel.getRows()*kernel.getCols() == 0)
    return;

  if (mode == "valid") {
    if (kernel.getRows() > M.getRows() || kernel.getCols() > M.getCols())
      return;
  }

  vpMatrix M_padded, res_same;

  if (mode == "full" || mode == "same") {
    const unsigned int pad_x = kernel.getCols()-1;
    const unsigned int pad_y = kernel.getRows()-1;
    M_padded.resize(M.getRows() + 2*pad_y, M.getCols() + 2*pad_x, true, false);
    M_padded.insert(M, pad_y, pad_x);

    if (mode == "same") {
      res.resize(M.getRows(), M.getCols(), false, false);
      res_same.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
    } else {
      res.resize(M.getRows() + pad_y, M.getCols() + pad_x, true, false);
    }
  } else if (mode == "valid") {
    M_padded = M;
    res.resize(M.getRows()-kernel.getRows()+1, M.getCols()-kernel.getCols()+1);
  } else {
    return;
  }

  if (mode == "same") {
    for (unsigned int i = 0; i < res_same.getRows(); i++) {
      for (unsigned int j = 0; j < res_same.getCols(); j++) {
        for (unsigned int k = 0; k < kernel.getRows(); k++) {
          for (unsigned int l = 0; l < kernel.getCols(); l++) {
            res_same[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
          }
        }
      }
    }

    const unsigned int start_i = kernel.getRows()/2;
    const unsigned int start_j = kernel.getCols()/2;
    for (unsigned int i = 0; i < M.getRows(); i++) {
      memcpy(res.data + i*M.getCols(), res_same.data + (i+start_i)*res_same.getCols() + start_j, sizeof(double)*M.getCols());
    }
  } else {
    for (unsigned int i = 0; i < res.getRows(); i++) {
      for (unsigned int j = 0; j < res.getCols(); j++) {
        for (unsigned int k = 0; k < kernel.getRows(); k++) {
          for (unsigned int l = 0; l < kernel.getCols(); l++) {
            res[i][j] += M_padded[i+k][j+l] * kernel[kernel.getRows()-k-1][kernel.getCols()-l-1];
          }
        }
      }
    }
  }
}

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

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

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

#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)