vpMatrix.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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/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>
#endif
#endif
 
BEGIN_VISP_NAMESPACE
 
#ifdef VISP_HAVE_LAPACK
#ifdef VISP_HAVE_GSL
#elif defined(VISP_HAVE_MKL)
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);
 
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);
}
 
vpMatrix::vpMatrix(const vpHomogeneousMatrix &M)
{
  *this = M;
}
 
vpMatrix::vpMatrix(const vpVelocityTwistMatrix &V)
{
  *this = V;
}
 
vpMatrix::vpMatrix(const vpForceTwistMatrix &F)
{
  *this = F;
}
 
vpMatrix::vpMatrix(const vpRotationMatrix &R)
{
  *this = R;
}
 
vpMatrix::vpMatrix(const vpColVector &v)
{
  *this = v;
}
 
vpMatrix::vpMatrix(const vpRowVector &v)
{
  *this = v;
}
 
vpMatrix::vpMatrix(const vpTranslationVector &t)
{
  *this = t;
}
 
#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;
}
 
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::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::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) {
    unsigned int k = 0;
    for (unsigned int j = 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) {
    unsigned int k = 0;
    for (unsigned int j = 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::cholesky() const
{
#if defined(VISP_HAVE_EIGEN3)
  return choleskyByEigen3();
#elif defined(VISP_HAVE_LAPACK)
  return choleskyByLapack();
#elif defined(VISP_HAVE_OPENCV)
  return choleskyByOpenCV();
#else
  throw(vpException(vpException::fatalError, "Cannot compute matrix Chloesky decomposition. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}
 
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(); }
 
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)
 
END_VISP_NAMESPACE