vpMatrix_lu.cpp

/****************************************************************************
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2017 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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>

#ifndef DOXYGEN_SHOULD_SKIP_THIS

#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>

// Exception
#include <visp3/core/vpException.h>
#include <visp3/core/vpMatrixException.h>

// Debug trace
#include <visp3/core/vpDebug.h>

#include <cmath>    // std::fabs
#include <limits>   // numeric_limits

#define TINY 1.0e-20;


/*--------------------------------------------------------------------
  LU Decomposition  related functions
-------------------------------------------------------------------- */

void
vpMatrix::LUDcmp(unsigned int *perm, int& d)
{
  unsigned int n = rowNum;

  unsigned int i,imax=0,j,k;
  double big,dum,sum_,temp;
  vpColVector vv(n);

  d=1;
  for (i=0;i<n;i++) {
    big=0.0;
    for (j=0;j<n;j++)
      if ((temp=fabs(rowPtrs[i][j])) > big) big=temp;
    //if (big == 0.0)
    if (std::fabs(big) <= std::numeric_limits<double>::epsilon())
    {
      //vpERROR_TRACE("Singular vpMatrix in  LUDcmp") ;
      throw(vpMatrixException(vpMatrixException::matrixError,
                              "Singular vpMatrix in  LUDcmp")) ;
    }
    vv[i]=1.0/big;
  }
  for (j=0;j<n;j++) {
    for (i=0;i<j;i++) {
      sum_=rowPtrs[i][j];
      for (k=0;k<i;k++) sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
      rowPtrs[i][j]=sum_;
    }
    big=0.0;
    for (i=j;i<n;i++) {
      sum_=rowPtrs[i][j];
      for (k=0;k<j;k++)
        sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
      rowPtrs[i][j]=sum_;
      if ( (dum=vv[i]*fabs(sum_)) >= big) {
        big=dum;
        imax=i;
      }
    }
    if (j != imax) {
      for (k=0;k<n;k++) {
        dum=rowPtrs[imax][k];
        rowPtrs[imax][k]=rowPtrs[j][k];
        rowPtrs[j][k]=dum;
      }
      d *= -1;
      vv[imax]=vv[j];
    }
    perm[j]=imax;
    //if (rowPtrs[j][j] == 0.0)
    if (std::fabs(rowPtrs[j][j]) <= std::numeric_limits<double>::epsilon())
      rowPtrs[j][j]=TINY;
    if (j != n) {
      dum=1.0/(rowPtrs[j][j]);
      for (i=j+1;i<n;i++) rowPtrs[i][j] *= dum;
    }
  }
}

#undef TINY

void vpMatrix::LUBksb(unsigned int *perm, vpColVector& b)
{
  unsigned int n = rowNum;

  unsigned int ii=0;
  double sum_;
  bool flag = false;
  unsigned int i;

  for (i=0;i<n;i++) {
    unsigned int ip=perm[i];
    sum_=b[ip];
    b[ip]=b[i];
    if (flag) {
      for (unsigned int j=ii;j<=i-1;j++) sum_ -= rowPtrs[i][j]*b[j];
    }
    //else if (sum_) {
    else if (std::fabs(sum_) > std::numeric_limits<double>::epsilon()) {
      ii=i;
      flag = true;
    }
    b[i]=sum_;
  }
  // for (int i=n-1;i>=0;i--) {
  //   sum_=b[i];
  //   for (int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
  //   b[i]=sum_/rowPtrs[i][i];
  // }
  i=n;
  do {
    i --;

    sum_=b[i];
    for (unsigned int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
    b[i]=sum_/rowPtrs[i][i];
  } while(i != 0);
}
#endif // doxygen should skip this

vpMatrix
vpMatrix::inverseByLU() const
{
  unsigned int i,j;

  if ( rowNum != colNum)
  {
    vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
    throw(vpMatrixException(vpMatrixException::matrixError,
                "Cannot invert a non-square vpMatrix")) ;
  }

  vpMatrix B(rowNum, rowNum), X(rowNum, rowNum);
  vpMatrix V(rowNum, rowNum);
  vpColVector W(rowNum);

  for (i=0; i<rowNum; i++) {
    for (j=0; j<rowNum; j++) {
      B[i][j] = (i == j) ? 1 : 0;
    }
  }

  vpMatrix A(rowNum, rowNum);
  A = *this;

  unsigned int *perm = new unsigned int[rowNum];

  try {
    int p;
    A.LUDcmp(perm, p);
  }
  catch(vpException &e) {
    delete [] perm;
    throw(e);
  }

  vpColVector c_tmp(rowNum)  ;
  for (j=1; j<=rowNum; j++)
  {
    c_tmp =0 ;  c_tmp[j-1] = 1 ;
    A.LUBksb(perm, c_tmp);
    for (unsigned int k=0 ; k < c_tmp.getRows() ; k++)
      B[k][j-1] = c_tmp[k] ;
  }
  delete [] perm;
  return B;
}