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_GSL
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#endif

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

#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 defined(VISP_HAVE_LAPACK)
  return inverseByLULapack();
#elif defined(VISP_HAVE_EIGEN3)
  return inverseByLUEigen3();
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
  return inverseByLUOpenCV();
#elif defined(VISP_HAVE_GSL)
  return inverseByLUGsl();
#else
  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Eigen3, "
                                             "Lapack, OpenCV or GSL 3rd party"));
#endif
}

double vpMatrix::detByLU() const
{
  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();
#elif defined(VISP_HAVE_GSL)
    return detByLUGsl();
#else
    throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Lapack, "
                                               "Eigen3, OpenCV or GSL 3rd party"));
#endif
  }
}


#if defined(VISP_HAVE_GSL)

vpMatrix vpMatrix::inverseByLUGsl() const
{
  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;
}

double vpMatrix::detByLUGsl() const
{
  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;
}
#endif

#ifdef VISP_HAVE_LAPACK

vpMatrix vpMatrix::inverseByLULapack() const
{
  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;
}

double vpMatrix::detByLULapack() const
{
  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

#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