vpMatrix.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 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 https://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.
 */
 
#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/vpCPUFeatures.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.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>
 
#if defined(VISP_HAVE_SIMDLIB)
#include <Simd/SimdLib.h>
#endif
 
#ifdef VISP_HAVE_LAPACK
#ifdef VISP_HAVE_GSL
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#elif defined(VISP_HAVE_MKL)
#include <mkl.h>
typedef MKL_INT integer;
 
void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
                          double *work_data, int lwork_, int &info_)
{
  MKL_INT n = static_cast<MKL_INT>(n_);
  MKL_INT lda = static_cast<MKL_INT>(lda_);
  MKL_INT lwork = static_cast<MKL_INT>(lwork_);
  MKL_INT info = static_cast<MKL_INT>(info_);
 
  dsyev(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
}
 
#else
#if defined(VISP_HAVE_LAPACK_BUILT_IN)
typedef long int integer;
#else
typedef int integer;
#endif
extern "C" integer dsyev_(char *jobz, char *uplo, integer *n, double *a, integer *lda, double *w, double *WORK,
                          integer *lwork, integer *info);
 
void vpMatrix::blas_dsyev(char jobz, char uplo, unsigned int n_, double *a_data, unsigned int lda_, double *w_data,
                          double *work_data, int lwork_, int &info_)
{
  integer n = static_cast<integer>(n_);
  integer lda = static_cast<integer>(lda_);
  integer lwork = static_cast<integer>(lwork_);
  integer info = static_cast<integer>(info_);
 
  dsyev_(&jobz, &uplo, &n, a_data, &lda, w_data, work_data, &lwork, &info);
 
  lwork_ = static_cast<int>(lwork);
  info_ = static_cast<int>(info);
}
#endif
#endif
 
#if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
const unsigned int vpMatrix::m_lapack_min_size_default = 0;
unsigned int vpMatrix::m_lapack_min_size = vpMatrix::m_lapack_min_size_default;
#endif
 
// Prototypes of specific functions
vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row);
 
void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
                            unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
                            vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
{
  Ap.resize(ncols, nrows, true, false);
 
  // compute the highest singular value and the rank of h
  double maxsv = sv[0];
 
  rank_out = 0;
 
  unsigned int sv_size = sv.size();
  for (unsigned int i = 0; i < sv_size; ++i) {
    if (sv[i] > (maxsv * svThreshold)) {
      rank_out++;
    }
  }
 
  unsigned int rank = static_cast<unsigned int>(rank_out);
  if (rank_in) {
    rank = static_cast<unsigned int>(*rank_in);
  }
 
  for (unsigned int i = 0; i < ncols; ++i) {
    for (unsigned int j = 0; j < nrows; ++j) {
      for (unsigned int k = 0; k < rank; ++k) {
        Ap[i][j] += (V[i][k] * U[j][k]) / sv[k];
      }
    }
  }
 
  // Compute im(A)
  if (imA) {
    imA->resize(nrows, rank);
 
    for (unsigned int i = 0; i < nrows; ++i) {
      for (unsigned int j = 0; j < rank; ++j) {
        (*imA)[i][j] = U[i][j];
      }
    }
  }
 
  // Compute im(At)
  if (imAt) {
    imAt->resize(ncols, rank);
    for (unsigned int i = 0; i < ncols; ++i) {
      for (unsigned int j = 0; j < rank; ++j) {
        (*imAt)[i][j] = V[i][j];
      }
    }
  }
 
  // Compute ker(At)
  if (kerAt) {
    kerAt->resize(ncols - rank, ncols);
    if (rank != ncols) {
      unsigned int v_rows = V.getRows();
      for (unsigned int k = 0; k < (ncols - rank); ++k) {
        unsigned j = k + rank;
        for (unsigned int i = 0; i < v_rows; ++i) {
          (*kerAt)[k][i] = V[i][j];
        }
      }
    }
  }
}
 
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);
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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 = nullptr;
  A.dsize = 0;
  A.data = nullptr;
}
 
vpMatrix::vpMatrix(const std::initializer_list<double> &list) : vpArray2D<double>(list) { }
 
vpMatrix::vpMatrix(unsigned int nrows, unsigned int ncols, const std::initializer_list<double> &list)
  : vpArray2D<double>(nrows, ncols, list)
{ }
 
vpMatrix::vpMatrix(const std::initializer_list<std::initializer_list<double> > &lists) : vpArray2D<double>(lists) { }
#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 == nullptr) { // 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;
  M.resize(nrows, ncols, false, false);
  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 { return transpose(); }
 
vpMatrix vpMatrix::transpose() const
{
  vpMatrix At;
  transpose(At);
  return At;
}
 
void vpMatrix::transpose(vpMatrix &At) const
{
  At.resize(colNum, rowNum, false, false);
 
  if ((rowNum <= 16) || (colNum <= 16)) {
    for (unsigned int i = 0; i < rowNum; ++i) {
      for (unsigned int j = 0; j < colNum; ++j) {
        At[j][i] = (*this)[i][j];
      }
    }
  }
  else {
#if defined(VISP_HAVE_SIMDLIB)
    SimdMatTranspose(data, rowNum, colNum, At.data);
#else
    // https://stackoverflow.com/a/21548079
    const int tileSize = 32;
    for (unsigned int i = 0; i < rowNum; i += tileSize) {
      for (unsigned int j = 0; j < colNum; j++) {
        for (unsigned int b = 0; b < static_cast<unsigned int>(tileSize) && i + b < rowNum; b++) {
          At[j][i + b] = (*this)[i + b][j];
        }
      }
    }
#endif
  }
}
 
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);
  }
 
  // If available use Lapack only for large matrices
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char transa = 't';
    const char transb = 'n';
 
    vpMatrix::blas_dgemm(transa, transb, rowNum, rowNum, colNum, alpha, data, colNum, data, colNum, beta, B.data,
                         rowNum);
#endif
  }
  else {
    // 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 available use Lapack only for large matrices
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char transa = 'n';
    const char transb = 't';
 
    vpMatrix::blas_dgemm(transa, transb, colNum, colNum, rowNum, alpha, data, colNum, data, colNum, beta, B.data,
                         colNum);
#endif
  }
  else {
    for (unsigned int i = 0; i < colNum; ++i) {
      double *Bi = B[i];
      for (unsigned int j = 0; j < i; ++j) {
        double *ptr = data;
        double s = 0;
        for (unsigned int k = 0; k < rowNum; ++k) {
          s += (*(ptr + i)) * (*(ptr + j));
          ptr += colNum;
        }
        *Bi++ = s;
        B[j][i] = s;
      }
      double *ptr = data;
      double s = 0;
      for (unsigned int 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)
{
  resize(A.getRows(), A.getCols(), false, false);
 
  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
    memcpy(data, A.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const vpMatrix &A)
{
  resize(A.getRows(), A.getCols(), false, false);
 
  if ((data != nullptr) && (A.data != nullptr) && (data != A.data)) {
    memcpy(data, A.data, dsize * sizeof(double));
  }
 
  return *this;
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
vpMatrix &vpMatrix::operator=(vpMatrix &&other)
{
  if (this != &other) {
    if (data) {
      free(data);
    }
    if (rowPtrs) {
      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 = nullptr;
    other.dsize = 0;
    other.data = nullptr;
  }
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const std::initializer_list<double> &list)
{
  if (dsize != static_cast<unsigned int>(list.size())) {
    resize(1, static_cast<unsigned int>(list.size()), false, false);
  }
 
  std::copy(list.begin(), list.end(), data);
 
  return *this;
}
 
vpMatrix &vpMatrix::operator=(const std::initializer_list<std::initializer_list<double> > &lists)
{
  unsigned int nrows = static_cast<unsigned int>(lists.size()), ncols = 0;
  for (auto &l : lists) {
    if (static_cast<unsigned int>(l.size()) > ncols) {
      ncols = static_cast<unsigned int>(l.size());
    }
  }
 
  resize(nrows, ncols, false, false);
  auto it = lists.begin();
  for (unsigned int i = 0; i < rowNum; ++i, ++it) {
    std::copy(it->begin(), it->end(), rowPtrs[i]);
  }
 
  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;
}
 
vpMatrix &vpMatrix::operator<<(double val)
{
  resize(1, 1, false, false);
  rowPtrs[0][0] = val;
  return *this;
}
 
vpMatrix &vpMatrix::operator,(double val)
{
  resize(1, colNum + 1, false, false);
  rowPtrs[0][colNum - 1] = val;
  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, true);
 
  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 available use Lapack only for large matrices
  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    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);
#endif
  }
  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;
      }
    }
  }
}
 
//---------------------------------
// 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 available use Lapack only for large matrices
  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
                    (B.colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const 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);
#endif
  }
  else {
    // 5/12/06 some "very" simple optimization to avoid indexation
    const unsigned int BcolNum = B.colNum;
    const unsigned int BrowNum = B.rowNum;
    double **BrowPtrs = B.rowPtrs;
    for (unsigned int i = 0; i < A.rowNum; ++i) {
      const double *rowptri = A.rowPtrs[i];
      double *ci = C[i];
      for (unsigned int j = 0; j < BcolNum; ++j) {
        double s = 0;
        for (unsigned int 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
  const unsigned int BcolNum = B.colNum;
  const unsigned int BrowNum = B.rowNum;
  double **BrowPtrs = B.rowPtrs;
  for (unsigned int i = 0; i < A.rowNum; ++i) {
    const double *rowptri = A.rowPtrs[i];
    double *ci = C[i];
    for (unsigned int j = 0; j < BcolNum; ++j) {
      double s = 0;
      for (unsigned int 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()));
  }
  // Considering perfMatrixMultiplication.cpp benchmark,
  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
  // Lapack usage needs to be validated again.
  // If available use Lapack only for large matrices.
  // Using SSE2 doesn't speed up.
  bool useLapack = ((A.rowNum > vpMatrix::m_lapack_min_size) || (A.colNum > vpMatrix::m_lapack_min_size) ||
                    (B.colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const 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);
#endif
  }
  else {
    // 5/12/06 some "very" simple optimization to avoid indexation
    const unsigned int BcolNum = B.colNum;
    const unsigned int BrowNum = B.rowNum;
    double **BrowPtrs = B.rowPtrs;
    for (unsigned int i = 0; i < A.rowNum; ++i) {
      const double *rowptri = A.rowPtrs[i];
      double *ci = C[i];
      for (unsigned int j = 0; j < BcolNum; ++j) {
        double s = 0;
        for (unsigned int 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;
  C.resize(rowNum, 3, false, false);
 
  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 vpHomogeneousMatrix &M) const
{
  if (colNum != M.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (3x3) rotation matrix", rowNum,
                      colNum));
  }
  vpMatrix C;
  C.resize(rowNum, 4, false, false);
 
  const unsigned int McolNum = M.getCols();
  const unsigned int MrowNum = M.getRows();
  for (unsigned int i = 0; i < rowNum; ++i) {
    const double *rowptri = rowPtrs[i];
    double *ci = C[i];
    for (unsigned int j = 0; j < McolNum; ++j) {
      double s = 0;
      for (unsigned int k = 0; k < MrowNum; ++k) {
        s += rowptri[k] * M[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);
 
  // Considering perfMatrixMultiplication.cpp benchmark,
  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
  // Lapack usage needs to be validated again.
  // If available use Lapack only for large matrices.
  // Speed up obtained using SSE2.
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
                    (V.colNum > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char trans = 'n';
    vpMatrix::blas_dgemm(trans, trans, V.colNum, rowNum, colNum, alpha, V.data, V.colNum, data, colNum, beta, M.data,
                         M.colNum);
#endif
  }
  else {
#if defined(VISP_HAVE_SIMDLIB)
    SimdMatMulTwist(data, rowNum, V.data, M.data);
#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;
  M.resize(rowNum, 6, false, false);
 
  // Considering perfMatrixMultiplication.cpp benchmark,
  // using either MKL, OpenBLAS, or Netlib can slow down this function with respect to the naive code.
  // Lapack usage needs to be validated again.
  // If available use Lapack only for large matrices.
  // Speed up obtained using SSE2.
  bool useLapack = ((rowNum > vpMatrix::m_lapack_min_size) || (colNum > vpMatrix::m_lapack_min_size) ||
                    (V.getCols() > vpMatrix::m_lapack_min_size));
#if !(defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL))
  useLapack = false;
#endif
 
  if (useLapack) {
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN) && !defined(VISP_HAVE_GSL)
    const double alpha = 1.0;
    const double beta = 0.0;
    const char trans = 'n';
    vpMatrix::blas_dgemm(trans, trans, V.getCols(), rowNum, colNum, alpha, V.data, V.getCols(), data, colNum, beta,
                         M.data, M.colNum);
#endif
  }
  else {
#if defined(VISP_HAVE_SIMDLIB)
    SimdMatMulTwist(data, rowNum, V.data, M.data);
#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;
}
 
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 subtract (%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 subtract (%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 subtract (%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;
 
  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)
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return B;
  }
 
  unsigned int Brow = B.getRows();
  unsigned int Bcol = B.getCols();
 
  vpMatrix C;
  C.resize(Brow, Bcol, false, false);
 
  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
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  vpMatrix M;
  M.resize(rowNum, colNum, false, false);
 
  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
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  if (std::fabs(x) < std::numeric_limits<double>::epsilon()) {
    throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
  }
 
  vpMatrix C;
  C.resize(rowNum, colNum, false, false);
 
  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)
{
  if (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  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 (std::fabs(x - 1.) < std::numeric_limits<double>::epsilon()) {
    return *this;
  }
 
  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);
  }
 
  memcpy(out.data, data, sizeof(double) * out.getCols());
}
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, false);
 
#if defined(VISP_HAVE_SIMDLIB)
  SimdVectorHadamard(data, m.data, dsize, out.data);
#else
  for (unsigned int i = 0; i < dsize; ++i) {
    out.data[i] = data[i] * m.data[i];
  }
#endif
 
  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();
 
  out.resize(r1 * r2, c1 * c2, false, false);
 
  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;
  out.resize(r1 * r2, c1 * c2, false, false);
 
  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 defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  svdOpenCV(w, V);
#else
  (void)w;
  (void)V;
  throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3 or OpenCV 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 defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, svThreshold);
#else
  (void)Ap;
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, rank_in);
#else
  (void)Ap;
  (void)rank_in;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 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 defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(svThreshold);
#else
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
vpMatrix vpMatrix::pseudoInverse(int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(rank_in);
#else
  (void)rank_in;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#if defined(VISP_HAVE_LAPACK)
vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
 
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
 
  vpMatrix U, V;
  vpColVector sv;
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
 
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
 
unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
 
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
 
  return static_cast<unsigned int>(rank_out);
}
 
vpMatrix vpMatrix::pseudoInverseLapack(int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
 
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
 
  vpMatrix U, V;
  vpColVector sv;
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
                                  vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
 
  U.svdLapack(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
 
  return rank_out;
}
 
#endif
#if defined(VISP_HAVE_EIGEN3)
vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
  vpColVector sv;
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
 
unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
 
unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
 
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
 
  return static_cast<unsigned int>(rank_out);
}
 
vpMatrix vpMatrix::pseudoInverseEigen3(int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
  vpColVector sv;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
                                  vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
  U.svdEigen3(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
 
  return rank_out;
}
 
#endif
#if defined(VISP_HAVE_OPENCV)
vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
  vpColVector sv;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, nullptr, nullptr, nullptr);
 
  return static_cast<unsigned int>(rank_out);
}
 
unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
                                           vpMatrix &imAt, vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
 
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, nullptr, &imA, &imAt, &kerAt);
 
  return static_cast<unsigned int>(rank_out);
}
 
vpMatrix vpMatrix::pseudoInverseOpenCV(int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V, Ap;
  vpColVector sv;
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return Ap;
}
 
int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
  vpColVector sv;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  Ap.resize(ncols, nrows, false, false);
 
  U = *this;
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, nullptr, nullptr, nullptr);
 
  return rank_out;
}
 
int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
                                  vpMatrix &kerAt) const
{
  unsigned int nrows = getRows();
  unsigned int ncols = getCols();
  int rank_out;
  double svThreshold = 1e-26;
  vpMatrix U, V;
 
  if (nrows < ncols) {
    U.resize(ncols, ncols, true);
    U.insert(*this, 0, 0);
  }
  else {
    U = *this;
  }
 
  U.svdOpenCV(sv, V);
 
  compute_pseudo_inverse(U, sv, V, nrows, ncols, svThreshold, Ap, rank_out, &rank_in, &imA, &imAt, &kerAt);
 
  return rank_out;
}
 
#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 defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, svThreshold);
#else
  (void)Ap;
  (void)sv;
  (void)svThreshold;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, rank_in);
#else
  (void)Ap;
  (void)sv;
  (void)rank_in;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 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);
}
 
int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
{
  vpMatrix kerAt;
  return pseudoInverse(Ap, sv, rank_in, 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 defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(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 or OpenCV 3rd party"));
#endif
}
 
int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
                            vpMatrix &kerAt) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
#else
  (void)Ap;
  (void)sv;
  (void)rank_in;
  (void)imA;
  (void)imAt;
  (void)kerAt;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
vpColVector vpMatrix::getCol(unsigned int j, unsigned int i_begin, 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(unsigned int j) const { return getCol(j, 0, rowNum); }
 
vpRowVector vpMatrix::getRow(unsigned int i) const { return getRow(i, 0, colNum); }
 
vpRowVector vpMatrix::getRow(unsigned int i, unsigned int j_begin, unsigned int row_size) const
{
  if (((j_begin + row_size) > colNum) || (i >= rowNum)) {
    throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
  }
  vpRowVector r(row_size);
  if ((r.data != nullptr) && (data != nullptr)) {
    memcpy(r.data, (*this)[i] + j_begin, row_size * sizeof(double));
  }
 
  return r;
}
 
vpColVector vpMatrix::getDiag() const
{
  unsigned int min_size = std::min<unsigned int>(rowNum, colNum);
  vpColVector diag;
 
  if (min_size > 0) {
    diag.resize(min_size, false);
 
    for (unsigned int i = 0; i < min_size; ++i) {
      diag[i] = (*this)[i][i];
    }
  }
 
  return diag;
}
 
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 != nullptr) && (A.data == C.data)) {
    std::cerr << "A and C must be two different objects!" << std::endl;
    return;
  }
 
  if ((B.data != nullptr) && (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 != nullptr) && (A.data != nullptr) && (A.size() > 0)) {
    // Copy A in C
    memcpy(C.data, A.data, sizeof(double) * A.size());
  }
 
  if ((C.data != nullptr) && (B.data != nullptr) && (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 != nullptr) && (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 != nullptr) && (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, unsigned int r, unsigned int c)
{
  vpArray2D<double> C;
 
  vpArray2D<double>::insert(A, B, C, r, c);
 
  return vpMatrix(C);
}
 
void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, unsigned int c)
{
  vpArray2D<double> C_array;
 
  vpArray2D<double>::insert(A, B, C_array, r, c);
 
  C = C_array;
}
 
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, const std::string &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.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);
      }
    }
  }
 
  size_type totalLength = length;
  // increase totalLength according to maxBefore
  totalLength = vpMath::maximum(totalLength, maxBefore);
  // decrease maxAfter according to totalLength
  maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
 
  if (!intro.empty()) {
    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(static_cast<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(static_cast<std::streamsize>(maxAfter));
          s << values[(i * n) + j].substr(p, maxAfter).c_str();
        }
        else {
          s.width(static_cast<std::streamsize>(maxAfter));
          s << ".0";
        }
      }
 
      s << ' ';
    }
    s << std::endl;
  }
 
  s.flags(original_flags); // restore s to standard state
 
  return static_cast<int>(maxBefore + maxAfter);
}
 
std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
{
  unsigned int this_rows = this->getRows();
  unsigned int this_col = this->getCols();
  os << "[ ";
  for (unsigned int i = 0; i < this_rows; ++i) {
    for (unsigned int j = 0; j < this_col; ++j) {
      os << (*this)[i][j] << ", ";
    }
    if (this->getRows() != (i + 1)) {
      os << ";" << std::endl;
    }
    else {
      os << "]" << std::endl;
    }
  }
  return os;
}
 
std::ostream &vpMatrix::maplePrint(std::ostream &os) const
{
  unsigned int this_rows = this->getRows();
  unsigned int this_col = this->getCols();
  os << "([ " << std::endl;
  for (unsigned int i = 0; i < this_rows; ++i) {
    os << "[";
    for (unsigned int j = 0; j < this_col; ++j) {
      os << (*this)[i][j] << ", ";
    }
    os << "]," << std::endl;
  }
  os << "])" << std::endl;
  return os;
}
 
std::ostream &vpMatrix::csvPrint(std::ostream &os) const
{
  unsigned int this_rows = this->getRows();
  unsigned int this_col = this->getCols();
  for (unsigned int i = 0; i < this_rows; ++i) {
    for (unsigned int j = 0; j < this_col; ++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;
 
  unsigned int this_rows = this->getRows();
  unsigned int this_col = this->getCols();
  for (unsigned int i = 0; i < this_rows; ++i) {
    for (unsigned int j = 0; j < this_col; ++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
            << static_cast<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 != nullptr) && (r.data != nullptr) && (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 != nullptr) && (tmp.data != nullptr) && (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, unsigned int r, unsigned int c)
{
  if (((r + A.getRows()) <= rowNum) && ((c + A.getCols()) <= colNum)) {
    if ((A.colNum == colNum) && (data != nullptr) && (A.data != nullptr) && (A.data != data)) {
      memcpy(data + (r * colNum), A.data, sizeof(double) * A.size());
    }
    else if ((data != nullptr) && (A.data != nullptr) && (A.data != data)) {
      unsigned int a_rows = A.getRows();
      for (unsigned int i = r; i < (r + a_rows); ++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
{
  vpColVector evalue(rowNum); // Eigen values
 
  if (rowNum != colNum) {
    throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
                      colNum));
  }
 
  // Check if the matrix is symmetric: 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) {
      // in comment: 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 symmetric matrix"));
      }
    }
  }
 
#if defined(VISP_HAVE_LAPACK)
#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
  {
    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);
  }
#else
  {
    const char jobz = 'N';
    const char uplo = 'U';
    vpMatrix A = (*this);
    vpColVector WORK;
    int lwork = -1;
    int info = 0;
    double wkopt;
    vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
    lwork = static_cast<int>(wkopt);
    WORK.resize(lwork);
    vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
  }
#endif
#else
  {
    throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
                      "You should install Lapack 3rd party"));
  }
#endif
  return evalue;
}
 
void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
{
  if (rowNum != colNum) {
    throw(vpException(vpException::dimensionError, "Cannot compute eigen values on a non square matrix (%dx%d)", rowNum,
                      colNum));
  }
 
  // Check if the matrix is symmetric: 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) {
      // -- in comment: 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 symmetric matrix"));
      }
    }
  }
 
  // Resize the output matrices
  evalue.resize(rowNum);
  evector.resize(rowNum, colNum);
 
#if defined(VISP_HAVE_LAPACK)
#if defined(VISP_HAVE_GSL) /* be careful of the copy below */
  {
    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  // defined(VISP_HAVE_GSL)
  {
    const char jobz = 'V';
    const char uplo = 'U';
    vpMatrix A = (*this);
    vpColVector WORK;
    int lwork = -1;
    int info = 0;
    double wkopt;
    vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, &wkopt, lwork, info);
    lwork = static_cast<int>(wkopt);
    WORK.resize(lwork);
    vpMatrix::blas_dsyev(jobz, uplo, rowNum, A.data, colNum, evalue.data, WORK.data, lwork, info);
    evector = A.t();
  }
#endif // defined(VISP_HAVE_GSL)
#else
  {
    throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. "
                      "You should install Lapack 3rd party"));
  }
#endif
}
 
unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
{
  unsigned int nbline = getRows();
  unsigned int nbcol = getCols();
 
  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
  vpColVector sv;
  sv.resize(nbcol, false);       // singular values
  V.resize(nbcol, nbcol, false); // 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, true);
  }
  else {
    U.resize(nbline, nbcol, false);
  }
 
  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 (sv[i] > maxsv) {
      maxsv = sv[i];
    }
  }
 
  unsigned int rank = 0;
  for (unsigned int i = 0; i < nbcol; ++i) {
    if (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 ((sv[j] <= (maxsv * svThreshold)) &&
          (std::fabs(V.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon())) {
        unsigned int v_rows = V.getRows();
        for (unsigned int i = 0; i < v_rows; ++i) {
          kerAt[k][i] = V[i][j];
        }
        k++;
      }
    }
  }
 
  return rank;
}
 
unsigned int vpMatrix::nullSpace(vpMatrix &kerA, double svThreshold) const
{
  unsigned int nbrow = getRows();
  unsigned int nbcol = getCols();
 
  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
  vpColVector sv;
  sv.resize(nbcol, false);       // singular values
  V.resize(nbcol, nbcol, false); // 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 (nbrow < nbcol) {
    U.resize(nbcol, nbcol, true);
  }
  else {
    U.resize(nbrow, nbcol, false);
  }
 
  U.insert(*this, 0, 0);
 
  U.svd(sv, V);
 
  // Compute the highest singular value and rank of the matrix
  double maxsv = sv[0];
 
  unsigned int rank = 0;
  for (unsigned int i = 0; i < nbcol; ++i) {
    if (sv[i] > (maxsv * svThreshold)) {
      rank++;
    }
  }
 
  kerA.resize(nbcol, nbcol - rank);
  if (rank != nbcol) {
    for (unsigned int j = 0, k = 0; j < nbcol; ++j) {
      // if( v.col(j) in kernel and non zero )
      if (sv[j] <= (maxsv * svThreshold)) {
        for (unsigned int i = 0; i < nbcol; ++i) {
          kerA[i][k] = V[i][j];
        }
        k++;
      }
    }
  }
 
  return (nbcol - rank);
}
 
unsigned int vpMatrix::nullSpace(vpMatrix &kerA, int dim) const
{
  unsigned int nbrow = getRows();
  unsigned int nbcol = getCols();
  unsigned int dim_ = static_cast<unsigned int>(dim);
 
  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
  vpColVector sv;
  sv.resize(nbcol, false);       // singular values
  V.resize(nbcol, nbcol, false); // 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 (nbrow < nbcol) {
    U.resize(nbcol, nbcol, true);
  }
  else {
    U.resize(nbrow, nbcol, false);
  }
 
  U.insert(*this, 0, 0);
 
  U.svd(sv, V);
 
  kerA.resize(nbcol, dim_);
  if (dim_ != 0) {
    unsigned int rank = nbcol - dim_;
    for (unsigned int k = 0; k < dim_; ++k) {
      unsigned int j = k + rank;
      for (unsigned int i = 0; i < nbcol; ++i) {
        kerA[i][k] = V[i][j];
      }
    }
  }
 
  double maxsv = sv[0];
  unsigned int rank = 0;
  for (unsigned int i = 0; i < nbcol; ++i) {
    if (sv[i] > (maxsv * 1e-6)) {
      rank++;
    }
  }
  return (nbcol - 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;
    expA.resize(rowNum, colNum, false);
    memcpy(expA.data, b, size_ * sizeof(double));
 
    delete[] b;
    return expA;
#else
    vpMatrix v_expE(rowNum, colNum, false);
    vpMatrix v_expD(rowNum, colNum, false);
    vpMatrix v_expX(rowNum, colNum, false);
    vpMatrix v_expcX(rowNum, colNum, false);
    vpMatrix v_eye(rowNum, colNum, false);
 
    v_eye.eye();
    vpMatrix exp(*this);
 
    //   -- in comment:   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);
    // -- in comment: double s = (0 > e+1)?0:e+1;
    double s = e + 1;
 
    double sca = 1.0 / pow(2.0, s);
    exp = sca * exp;
    v_expX = *this;
    v_expE = (c * exp) + v_eye;
    v_expD = (-c * exp) + v_eye;
    for (int k = 2; k <= q; ++k) {
      c = (c * (static_cast<double>((q - k) + 1))) / (static_cast<double>(k * (((2 * q) - k) + 1)));
      v_expcX = exp * v_expX;
      v_expX = v_expcX;
      v_expcX = c * v_expX;
      v_expE = v_expE + v_expcX;
      if (p) {
        v_expD = v_expD + v_expcX;
      }
      else {
        v_expD = v_expD - v_expcX;
      }
      p = !p;
    }
    v_expX = v_expD.inverseByLU();
    exp = v_expX * v_expE;
    for (int k = 1; k <= s; ++k) {
      v_expE = exp * exp;
      exp = v_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_comp.resize(M.getRows() - 1, M.getCols() - 1, false);
 
  unsigned int m_rows = M.getRows();
  unsigned int m_col = M.getCols();
  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_rows; ++j) {
      M_comp[i][j - 1] = M[i][j];
    }
  }
  for (unsigned int i = col + 1; i < m_col; ++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_rows; ++j) {
      M_comp[i - 1][j - 1] = M[i][j];
    }
  }
  return M_comp;
}
 
double vpMatrix::cond(double svThreshold) const
{
  unsigned int nbline = getRows();
  unsigned int nbcol = getCols();
 
  vpMatrix U, V; // Copy of the matrix, SVD function is destructive
  vpColVector sv;
  sv.resize(nbcol);              // singular values
  V.resize(nbcol, nbcol, false); // 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, true);
  }
  else {
    U.resize(nbline, nbcol, false);
  }
 
  U.insert(*this, 0, 0);
 
  U.svd(sv, V);
 
  // Compute the highest singular value
  double maxsv = 0;
  for (unsigned int i = 0; i < nbcol; ++i) {
    if (sv[i] > maxsv) {
      maxsv = sv[i];
    }
  }
 
  // Compute the rank of the matrix
  unsigned int rank = 0;
  for (unsigned int i = 0; i < nbcol; ++i) {
    if (sv[i] > (maxsv * svThreshold)) {
      rank++;
    }
  }
 
  // Compute the lowest singular value
  double minsv = maxsv;
  for (unsigned int i = 0; i < rank; ++i) {
    if (sv[i] < minsv) {
      minsv = sv[i];
    }
  }
 
  if (std::fabs(minsv) > std::numeric_limits<double>::epsilon()) {
    return maxsv / minsv;
  }
  else {
    return std::numeric_limits<double>::infinity();
  }
}
 
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 = H;
  unsigned int h_col = H.getCols();
  for (unsigned int i = 0; i < h_col; ++i) {
    HLM[i][i] += alpha * H[i][i];
  }
}
 
double vpMatrix::frobeniusNorm() 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::inducedL2Norm() const
{
  if (this->dsize != 0) {
    vpMatrix v;
    vpColVector w;
 
    vpMatrix M = *this;
 
    M.svd(w, v);
 
    double max = w[0];
    unsigned int maxRank = std::min<unsigned int>(this->getCols(), this->getRows());
    // The maximum reachable rank is either the number of columns or the number of rows
    // of the matrix.
    unsigned int boundary = std::min<unsigned int>(maxRank, w.size());
    // boundary is here to ensure that the number of singular values used for the com-
    // putation of the euclidean norm of the matrix is not greater than the maximum
    // reachable rank. Indeed, some svd library pad the singular values vector with 0s
    // if the input matrix is non-square.
    for (unsigned int i = 0; i < boundary; ++i) {
      if (max < w[i]) {
        max = w[i];
      }
    }
    return max;
  }
  else {
    return 0.;
  }
}
 
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;
}
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
double vpMatrix::euclideanNorm() const { return frobeniusNorm(); }
 
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(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(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)