vpMatrix_lu.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Matrix LU decomposition.
 *
*****************************************************************************/
 
#include <visp3/core/vpConfig.h>
 
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
 
#ifdef VISP_HAVE_EIGEN3
#include <Eigen/LU>
#endif
 
#ifdef VISP_HAVE_LAPACK
#ifdef VISP_HAVE_GSL
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#endif
#ifdef VISP_HAVE_MKL
#include <mkl.h>
typedef MKL_INT integer;
#else
#ifdef VISP_HAVE_LAPACK_BUILT_IN
typedef long int integer;
#else
typedef int integer;
#endif
extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
                        integer *info);
#endif
#endif
 
#if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
#include <opencv2/core/core.hpp>
#endif
 
// Exception
#include <visp3/core/vpException.h>
#include <visp3/core/vpMatrixException.h>
 
#include <cmath>  // std::fabs
#include <limits> // numeric_limits
 
/*--------------------------------------------------------------------
  LU Decomposition  related functions
-------------------------------------------------------------------- */
 
vpMatrix vpMatrix::inverseByLU() const
{
  if ((colNum == 1) && (rowNum == 1)) {
    vpMatrix inv;
    inv.resize(1, 1, false);
    double d = det();
    if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
      throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
                        rowNum, colNum));
    }
    inv[0][0] = 1. / d;
    return inv;
  }
  else if ((colNum == 2) && (rowNum == 2)) {
    vpMatrix inv;
    inv.resize(2, 2, false);
    double d = det();
    if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
      throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
                        rowNum, colNum));
    }
    d = 1. / d;
    inv[1][1] = (*this)[0][0] * d;
    inv[0][0] = (*this)[1][1] * d;
    inv[0][1] = -(*this)[0][1] * d;
    inv[1][0] = -(*this)[1][0] * d;
    return inv;
  }
  else if ((colNum == 3) && (rowNum == 3)) {
    vpMatrix inv;
    inv.resize(3, 3, false);
    double d = det();
    if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
      throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
                        rowNum, colNum));
    }
    d = 1. / d;
    inv[0][0] = (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1])) * d;
    inv[0][1] = (((*this)[0][2] * (*this)[2][1]) - ((*this)[0][1] * (*this)[2][2])) * d;
    inv[0][2] = (((*this)[0][1] * (*this)[1][2]) - ((*this)[0][2] * (*this)[1][1])) * d;
 
    inv[1][0] = (((*this)[1][2] * (*this)[2][0]) - ((*this)[1][0] * (*this)[2][2])) * d;
    inv[1][1] = (((*this)[0][0] * (*this)[2][2]) - ((*this)[0][2] * (*this)[2][0])) * d;
    inv[1][2] = (((*this)[0][2] * (*this)[1][0]) - ((*this)[0][0] * (*this)[1][2])) * d;
 
    inv[2][0] = (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0])) * d;
    inv[2][1] = (((*this)[0][1] * (*this)[2][0]) - ((*this)[0][0] * (*this)[2][1])) * d;
    inv[2][2] = (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0])) * d;
    return inv;
  }
  else {
#if defined(VISP_HAVE_LAPACK)
    return inverseByLULapack();
#elif defined(VISP_HAVE_EIGEN3)
    return inverseByLUEigen3();
#elif defined(VISP_HAVE_OPENCV)
    return inverseByLUOpenCV();
#else
    throw(vpException(vpException::fatalError, "Cannot inverse by LU. "
                      "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
  }
}
 
double vpMatrix::detByLU() const
{
  if ((rowNum == 1) && (colNum == 1)) {
    return (*this)[0][0];
  }
  else if ((rowNum == 2) && (colNum == 2)) {
    return (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0]));
  }
  else if ((rowNum == 3) && (colNum == 3)) {
    return ((((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1]))) -
             ((*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])))) +
            ((*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0]))));
  }
  else {
#if defined(VISP_HAVE_LAPACK)
    return detByLULapack();
#elif defined(VISP_HAVE_EIGEN3)
    return detByLUEigen3();
#elif defined(VISP_HAVE_OPENCV)
    return detByLUOpenCV();
#else
    throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
                      "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
  }
}
 
#if defined(VISP_HAVE_LAPACK)
vpMatrix vpMatrix::inverseByLULapack() const
{
#if defined(VISP_HAVE_GSL)
  {
    if (rowNum != colNum) {
      throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
    }
 
    gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
 
    // copy the input matrix to ensure the function doesn't modify its content
    unsigned int tda = static_cast<unsigned int>(A->tda);
    for (unsigned int i = 0; i < rowNum; ++i) {
      unsigned int k = i * tda;
      for (unsigned int j = 0; j < colNum; ++j)
        A->data[k + j] = (*this)[i][j];
    }
 
    vpMatrix Ainv(rowNum, colNum);
 
    gsl_matrix inverse;
    inverse.size1 = rowNum;
    inverse.size2 = colNum;
    inverse.tda = inverse.size2;
    inverse.data = Ainv.data;
    inverse.owner = 0;
    inverse.block = 0;
 
    gsl_permutation *p = gsl_permutation_alloc(rowNum);
    int s;
 
    // Do the LU decomposition on A and use it to solve the system
    gsl_linalg_LU_decomp(A, p, &s);
    gsl_linalg_LU_invert(A, p, &inverse);
 
    gsl_permutation_free(p);
    gsl_matrix_free(A);
 
    return Ainv;
  }
#else
  {
    if (rowNum != colNum) {
      throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
    }
 
    integer dim = (integer)rowNum;
    integer lda = dim;
    integer info;
    integer lwork = dim * dim;
    integer *ipiv = new integer[dim + 1];
    double *work = new double[lwork];
 
    vpMatrix A = *this;
 
    dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
    if (info) {
      delete[] ipiv;
      delete[] work;
      throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
    }
 
    dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
 
    delete[] ipiv;
    delete[] work;
 
    return A;
  }
#endif
}
 
double vpMatrix::detByLULapack() const
{
#if defined(VISP_HAVE_GSL)
  {
    double det = 0.;
 
    if (rowNum != colNum) {
      throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
                        rowNum, colNum));
    }
 
    gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
 
    // copy the input matrix to ensure the function doesn't modify its content
    unsigned int tda = (unsigned int)A->tda;
    for (unsigned int i = 0; i < rowNum; i++) {
      unsigned int k = i * tda;
      for (unsigned int j = 0; j < colNum; j++)
        A->data[k + j] = (*this)[i][j];
    }
 
    gsl_permutation *p = gsl_permutation_alloc(rowNum);
    int s;
 
    // Do the LU decomposition on A and use it to solve the system
    gsl_linalg_LU_decomp(A, p, &s);
    det = gsl_linalg_LU_det(A, s);
 
    gsl_permutation_free(p);
    gsl_matrix_free(A);
 
    return det;
  }
#else
  {
    if (rowNum != colNum) {
      throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
                        rowNum, colNum));
    }
 
    integer dim = (integer)rowNum;
    integer lda = dim;
    integer info;
    integer *ipiv = new integer[dim + 1];
 
    vpMatrix A = *this;
 
    dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
    if (info < 0) {
      delete[] ipiv;
      throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
    }
 
    double det = A[0][0];
    for (unsigned int i = 1; i < rowNum; ++i) {
      det *= A[i][i];
    }
 
    double sign = 1.;
    for (int i = 1; i <= dim; ++i) {
      if (ipiv[i] != i)
        sign = -sign;
    }
 
    det *= sign;
 
    delete[] ipiv;
 
    return det;
  }
#endif
}
#endif
 
#if defined(VISP_HAVE_OPENCV)
vpMatrix vpMatrix::inverseByLUOpenCV() const
{
  if (rowNum != colNum) {
    throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
  }
 
  cv::Mat M(rowNum, colNum, CV_64F, this->data);
 
  cv::Mat Minv = M.inv(cv::DECOMP_LU);
 
  vpMatrix A(rowNum, colNum);
  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
 
  return A;
}
 
double vpMatrix::detByLUOpenCV() const
{
  double det = 0.;
 
  if (rowNum != colNum) {
    throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
                      rowNum, colNum));
  }
 
  cv::Mat M(rowNum, colNum, CV_64F, this->data);
  det = cv::determinant(M);
 
  return (det);
}
#endif
 
#if defined(VISP_HAVE_EIGEN3)
 
vpMatrix vpMatrix::inverseByLUEigen3() const
{
  if (rowNum != colNum) {
    throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
  }
  vpMatrix A(this->getRows(), this->getCols());
 
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
                                                                                        this->getCols());
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
                                                                                         this->getCols());
 
  A_ = M.inverse();
 
  return A;
}
 
double vpMatrix::detByLUEigen3() const
{
  if (rowNum != colNum) {
    throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
                      rowNum, colNum));
  }
 
  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
                                                                                        this->getCols());
 
  return M.determinant();
}
#endif