vpMatrix_lu.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 LU decomposition.
 *
 * Authors:
 * Eric Marchand
 *
 *****************************************************************************/

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

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