vpMatrix_pseudo_inverse.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 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:
 * Pseudo inverse computation.
 */
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMatrix.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
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
 
void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
                            unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
                            vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt);
 
void compute_pseudo_inverse(const vpMatrix &U, const vpColVector &sv, const vpMatrix &V, unsigned int nrows,
                            unsigned int ncols, double svThreshold, vpMatrix &Ap, int &rank_out, int *rank_in,
                            vpMatrix *imA, vpMatrix *imAt, vpMatrix *kerAt)
{
  Ap.resize(ncols, nrows, true, false);
 
  // compute the highest singular value and the rank of h
  double maxsv = sv[0];
 
  rank_out = 0;
 
  unsigned int sv_size = sv.size();
  for (unsigned int i = 0; i < sv_size; ++i) {
    if (sv[i] >(maxsv * svThreshold)) {
      ++rank_out;
    }
  }
 
  unsigned int rank = static_cast<unsigned int>(rank_out);
  if (rank_in) {
    rank = static_cast<unsigned int>(*rank_in);
  }
 
  for (unsigned int i = 0; i < ncols; ++i) {
    for (unsigned int j = 0; j < nrows; ++j) {
      for (unsigned int k = 0; k < rank; ++k) {
        Ap[i][j] += (V[i][k] * U[j][k]) / sv[k];
      }
    }
  }
 
  // Compute im(A)
  if (imA) {
    imA->resize(nrows, rank);
 
    for (unsigned int i = 0; i < nrows; ++i) {
      for (unsigned int j = 0; j < rank; ++j) {
        (*imA)[i][j] = U[i][j];
      }
    }
  }
 
  // Compute im(At)
  if (imAt) {
    imAt->resize(ncols, rank);
    for (unsigned int i = 0; i < ncols; ++i) {
      for (unsigned int j = 0; j < rank; ++j) {
        (*imAt)[i][j] = V[i][j];
      }
    }
  }
 
  // Compute ker(At)
  if (kerAt) {
    kerAt->resize(ncols - rank, ncols);
    if (rank != ncols) {
      unsigned int v_rows = V.getRows();
      for (unsigned int k = 0; k < (ncols - rank); ++k) {
        unsigned j = k + rank;
        for (unsigned int i = 0; i < v_rows; ++i) {
          (*kerAt)[k][i] = V[i][j];
        }
      }
    }
  }
}
 
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS

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 defined(VISP_HAVE_OPENCV) // 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
}

int vpMatrix::pseudoInverse(vpMatrix &Ap, int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, rank_in);
#else
  (void)Ap;
  (void)rank_in;
  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 defined(VISP_HAVE_OPENCV) // 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
}

vpMatrix vpMatrix::pseudoInverse(int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(rank_in);
#else
  (void)rank_in;
  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) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, svThreshold);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, svThreshold);
#elif defined(VISP_HAVE_OPENCV) // 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
}

int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, rank_in);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, rank_in);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, rank_in);
#else
  (void)Ap;
  (void)sv;
  (void)rank_in;
  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);
}

int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt) const
{
  vpMatrix kerAt;
  return pseudoInverse(Ap, sv, rank_in, 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 defined(VISP_HAVE_OPENCV) // 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
}

int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt,
                            vpMatrix &kerAt) const
{
#if defined(VISP_HAVE_LAPACK)
  return pseudoInverseLapack(Ap, sv, rank_in, imA, imAt, kerAt);
#elif defined(VISP_HAVE_EIGEN3)
  return pseudoInverseEigen3(Ap, sv, rank_in, imA, imAt, kerAt);
#elif defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
  return pseudoInverseOpenCV(Ap, sv, rank_in, imA, imAt, kerAt);
#else
  (void)Ap;
  (void)sv;
  (void)rank_in;
  (void)imA;
  (void)imAt;
  (void)kerAt;
  throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
                    "Install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}

vpMatrix vpMatrix::dampedInverse(const double &ratioOfMaxSvd) const
{
  vpColVector w;
  vpMatrix V, Mcpy(*this);
  Mcpy.svd(w, V);
  double maxSingularValue = w.getMaxValue();
  vpMatrix I;
  I.eye(this->colNum);
  double lambda = ratioOfMaxSvd * maxSingularValue;
  vpMatrix dampedInv = ((lambda * I) + (this->transpose() * (*this))).inverseByLU()* this->transpose();
  return dampedInv;
}
 
END_VISP_NAMESPACE