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>
#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>

#ifdef VISP_HAVE_LAPACK
#  ifdef VISP_HAVE_GSL
#    include <gsl/gsl_eigen.h>
#    include <gsl/gsl_math.h>
#    include <gsl/gsl_linalg.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

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

#if defined __AVX__
#include <immintrin.h>
#define VISP_HAVE_AVX 1
#endif

#if VISP_HAVE_AVX
namespace {
  void transpose4x4(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
  {
    __m256d a0 = _mm256_loadu_pd(&a[i][j]);
    __m256d a1 = _mm256_loadu_pd(&a[i + 1][j]);
    __m256d a2 = _mm256_loadu_pd(&a[i + 2][j]);
    __m256d a3 = _mm256_loadu_pd(&a[i + 3][j]);

    __m256d T0 = _mm256_shuffle_pd(a0, a1, 15);
    __m256d T1 = _mm256_shuffle_pd(a0, a1, 0);
    __m256d T2 = _mm256_shuffle_pd(a2, a3, 15);
    __m256d T3 = _mm256_shuffle_pd(a2, a3, 0);

    a1 = _mm256_permute2f128_pd(T0, T2, 32);
    a3 = _mm256_permute2f128_pd(T0, T2, 49);
    a0 = _mm256_permute2f128_pd(T1, T3, 32);
    a2 = _mm256_permute2f128_pd(T1, T3, 49);

    _mm256_storeu_pd(&b[j][i], a0);
    _mm256_storeu_pd(&b[j + 1][i], a1);
    _mm256_storeu_pd(&b[j + 2][i], a2);
    _mm256_storeu_pd(&b[j + 3][i], a3);
  }

  void transpose16x16(const vpMatrix& a, vpMatrix& b, unsigned int i, unsigned int j)
  {
    transpose4x4(a, b, i, j);
    transpose4x4(a, b, i, j + 4);
    transpose4x4(a, b, i, j + 8);
    transpose4x4(a, b, i, j + 12);

    transpose4x4(a, b, i + 4, j);
    transpose4x4(a, b, i + 4, j + 4);
    transpose4x4(a, b, i + 4, j + 8);
    transpose4x4(a, b, i + 4, j + 12);

    transpose4x4(a, b, i + 8, j);
    transpose4x4(a, b, i + 8, j + 4);
    transpose4x4(a, b, i + 8, j + 8);
    transpose4x4(a, b, i + 8, j + 12);

    transpose4x4(a, b, i + 12, j);
    transpose4x4(a, b, i + 12, j + 4);
    transpose4x4(a, b, i + 12, j + 8);
    transpose4x4(a, b, i + 12, j + 12);
  }
}
#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;
  a1.resize(ncols, nrows, false, false);

  // 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, true);

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

#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 = NULL;
  A.dsize = 0;
  A.data = NULL;
}

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 == 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;
  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 {
    bool haveAVX = vpCPUFeatures::checkAVX();
#if !VISP_HAVE_AVX
    haveAVX = false;
#endif

    if (haveAVX) {
#if VISP_HAVE_AVX
      // matrix transpose using tiling
      const int nrows = static_cast<int>(rowNum);
      const int ncols = static_cast<int>(colNum);
      const int tileSize = 16;

      for (int i = 0; i < nrows;) {
        for (; i <= nrows - tileSize; i += tileSize) {
          int j = 0;
          for (; j <= ncols - tileSize; j += tileSize) {
            transpose16x16(*this, At, i, j);
          }

          for (int k = i; k < i + tileSize; k++) {
            for (int l = j; l < ncols; l++) {
              At[l][k] = (*this)[k][l];
            }
          }
        }

        for (; i < nrows; i++) {
          for (int j = 0; j < ncols; j++) {
            At[j][i] = (*this)[i][j];
          }
        }
      }

      _mm256_zeroupper();
#endif
    } 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];
          }
        }
      }
    }
  }
}

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 != NULL && A.data != NULL && data != A.data) {
    memcpy(data, A.data, dsize * sizeof(double));
  }

  return *this;
}

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
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;
}

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 {
    bool checkSSE2 = vpCPUFeatures::checkSSE2();
  #if !VISP_HAVE_SSE2
    checkSSE2 = false;
  #endif

    if (checkSSE2) {
  #if VISP_HAVE_SSE2
      vpMatrix V_trans;
      V_trans.resize(6, 6, false, false);
      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 {
      const unsigned int VcolNum = V.getCols();
      const unsigned int VrowNum = V.getRows();
      for (unsigned int i = 0; i < rowNum; i++) {
        const double *rowptri = rowPtrs[i];
        double *ci = M[i];
        for (unsigned int j = 0; j < VcolNum; j++) {
          double s = 0;
          for (unsigned int k = 0; k < VrowNum; k++)
            s += rowptri[k] * V[k][j];
          ci[j] = s;
        }
      }
    }
  }

  return M;
}

vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
{
  if (colNum != V.getRows()) {
    throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (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 {
    bool checkSSE2 = vpCPUFeatures::checkSSE2();
  #if !VISP_HAVE_SSE2
    checkSSE2 = false;
  #endif

    if (checkSSE2) {
  #if VISP_HAVE_SSE2
      vpMatrix V_trans;
      V_trans.resize(6, 6, false, false);
      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 {
      const unsigned int VcolNum = V.getCols();
      const unsigned int VrowNum = V.getRows();
      for (unsigned int i = 0; i < rowNum; i++) {
        const 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)
{
  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);

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

  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 (VISP_HAVE_OPENCV_VERSION >= 0x020101) // 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 (VISP_HAVE_OPENCV_VERSION >= 0x020101) // 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
}

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);
#else
  (void)svThreshold;
  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, ncols;
  unsigned int nrows_orig = getRows();
  unsigned int ncols_orig = getCols();

  vpMatrix Ap;
  Ap.resize(ncols_orig, nrows_orig, false, false);

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false, false);
  V.resize(ncols, ncols, false, false);
  vpColVector sv;
  sv.resize(ncols, false);

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

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false, false);
  V.resize(ncols, ncols, false, false);
  vpColVector sv;
  sv.resize(ncols, true);

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

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false, false);
  V.resize(ncols, ncols, false, false);
  sv.resize(ncols, false);

  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, true);
    sv.resize(nrows, false);
  } else {
    U.resize(nrows, ncols, false);
    sv.resize(ncols, false);
  }

  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;
  Ap.resize(ncols_orig, nrows_orig, false);

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false);
  V.resize(ncols, ncols, false);
  vpColVector sv;
  sv.resize(ncols, false);

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

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false);
  V.resize(ncols, ncols, false);
  vpColVector sv;
  sv.resize(ncols, false);

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

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false);
  V.resize(ncols, ncols, false);
  sv.resize(ncols, false);

  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, true);
    sv.resize(nrows, false);
  } else {
    U.resize(nrows, ncols, false);
    sv.resize(ncols, false);
  }

  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;
  Ap.resize(ncols_orig, nrows_orig, false);

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false);
  V.resize(ncols, ncols, false);
  vpColVector sv;
  sv.resize(ncols, false);

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

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

  vpMatrix U, V;
  U.resize(nrows, ncols, false);
  V.resize(ncols, ncols, false);
  vpColVector sv;
  sv.resize(ncols, false);

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

  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, true);
    sv.resize(nrows, false);
  } else {
    U.resize(nrows, ncols, false);
    sv.resize(ncols, false);
  }

  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
#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);
#else
  (void)Ap;
  (void)sv;
  (void)svThreshold;
  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);
}

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

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 != NULL && data != NULL) {
    memcpy(r.data, (*this)[i] + j_begin, row_size*sizeof(double));
  }

  return r;
}

vpColVector vpMatrix::getDiag() const
{
  unsigned int min_size = std::min(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 != 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, unsigned int r, unsigned int c)
{
  vpMatrix C;

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

  return C;
}

void vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, unsigned int r, 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, 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 <<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.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((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, unsigned int r, 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
{
  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 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"));
      }
    }
  }

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

#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;
    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 (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;
    expA.resize(rowNum, colNum, false);
    memcpy(expA.data, b, size_ * sizeof(double));

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

    _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_comp.resize(M.getRows() - 1, M.getCols() - 1, false);

  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(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 (fabs(sv[i]) > maxsv) {
      maxsv = fabs(sv[i]);
    }
  }

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

  // Compute the lowest singular value
  double minsv = maxsv;
  for (unsigned int i = 0; i < rank; i++) {
    if (fabs(sv[i]) < minsv) {
      minsv = fabs(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;
  for (unsigned int i = 0; i < H.getCols(); i++) {
    HLM[i][i] += alpha * H[i][i];
  }
}

vp_deprecated double vpMatrix::euclideanNorm() const
{
  return frobeniusNorm();
}

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

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