matrix_inverter.h

Go to the documentation of this file.

/*
 * Software License Agreement (Modified BSD License)
 *
 *  Copyright (c) 2011, PAL Robotics, S.L.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PAL Robotics, S.L. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

// BIG FAT NOTE: This file was initially intended to be part of a patchset for the KDL, but my views on how to implement IK
// building blocks have changed quite a bit since the initial implementation. However, the functionality provided by
// this file is still useful, and until I get the time to refactor this code, I'm stuck with it.

#ifndef KDL_MATRIXINVERTER_H
#define KDL_MATRIXINVERTER_H

// C++ standard headers
#include <algorithm>
#include <cassert>

// Boost headers
#include <boost/shared_ptr.hpp>

// Eigen headers
#include <Eigen/Dense>

// Forward declarations
namespace Eigen
{
namespace internal
{
template<typename MatrixType> class LsInverseReturnType;
template<typename MatrixType> class DlsInverseReturnType;
template<typename MatrixType> class LsSolveReturnType;
template<typename MatrixType> class DlsSolveReturnType;
} // internal
} // Eigen

namespace KDL
{


// TODO: A note on concurrency

// TODO: Doc solve() methods, remove weights from example

// TODO: Incorporate into JacobiSVD?

// TODO: Add isInitialized


template <typename MatrixType>
class MatrixInverter
{
public:
  typedef MatrixType Matrix;
  typedef boost::shared_ptr<Matrix> MatrixPtr;
  typedef typename Eigen::internal::plain_col_type<Matrix>::type Vector;
  typedef typename Matrix::Index Index;
  typedef typename Matrix::Scalar Scalar;
  typedef typename Eigen::JacobiSVD<Matrix, Eigen::ColPivHouseholderQRPreconditioner> SVD;
  typedef Eigen::internal::LsInverseReturnType<Matrix> LsInverseReturnType;
  typedef Eigen::internal::DlsInverseReturnType<Matrix> DlsInverseReturnType;
  typedef Eigen::internal::LsSolveReturnType<Matrix> LsSolveReturnType;
  typedef Eigen::internal::DlsSolveReturnType<Matrix> DlsSolveReturnType;

  MatrixInverter(const Index rows, const Index cols);

  MatrixInverter(const Matrix& A);

  MatrixInverter& compute(const Matrix& A);

  LsInverseReturnType inverse() {return lsInverse();}

  LsInverseReturnType lsInverse() {return LsInverseReturnType(*this);}

  DlsInverseReturnType dlsInverse() {return DlsInverseReturnType(*this);}

  LsSolveReturnType solve(const Vector& b) {return lsSolve(b);}

  LsSolveReturnType lsSolve(const Vector& b) {return LsSolveReturnType(b, *this);}

  DlsSolveReturnType dlsSolve(const Vector& b) {return DlsSolveReturnType(b, *this);}

  void setLsInverseThreshold(const Scalar eps) {lsEps_ = eps;}

  Scalar getLsInverseThreshold() const {return lsEps_;}

  void setDlsInverseThreshold(const Scalar dampEps) {dlsEps_ = dampEps;}

  Scalar getDlsInverseThreshold() const {return dlsEps_;}

  void setMaxDamping(const Scalar dampMax) {dampMax_ = dampMax;}

  Scalar getMaxDamping() const {return dampMax_;}

private:
  SVD       svd_;     
  Scalar    lsEps_;   
  Scalar    dlsEps_;  
  Scalar    dampMax_; 

  // Preallocated instances to allow execution in contexts where heap allocations are forbidden
  Matrix    pinv_;   
  Vector    solve1_; 
  Vector    solve2_; 

  void allocate(const Index rows, const Index cols);

  Index nonzeroSingularValues() const;

  Scalar computeDampingSq(const Vector& S) const;

  friend class Eigen::internal::LsInverseReturnType<MatrixType>;
  friend class Eigen::internal::DlsInverseReturnType<MatrixType>;
  friend class Eigen::internal::LsSolveReturnType<MatrixType>;
  friend class Eigen::internal::DlsSolveReturnType<MatrixType>;
};

template <typename MatrixType>
MatrixInverter<MatrixType>::MatrixInverter(const Index rows, const Index cols)
{
  allocate(rows, cols);
}

template <typename MatrixType>
MatrixInverter<MatrixType>::MatrixInverter(const Matrix& A)
{
  allocate(A.rows(), A.cols());
  compute(A);
}

template <typename MatrixType>
inline void MatrixInverter<MatrixType>::allocate(const Index rows, const Index cols)
{
  if (rows == 0 || cols == 0) throw; // TODO: Throw what? Decide when to throw, when to assert

  svd_     = SVD(rows, cols, Eigen::ComputeThinU | Eigen::ComputeThinV);
  lsEps_ = Eigen::NumTraits<Scalar>::epsilon();
  dlsEps_ = 1e3 * Eigen::NumTraits<Scalar>::epsilon(); // TODO: Is this a sensible default?
  dampMax_ = 0.1;                                       // TODO: Is this a sensible default?
  const Index minDim  = std::min(rows, cols);
  pinv_   = Matrix(minDim, minDim);
  solve1_ = Vector(minDim);
  solve2_ = Vector(minDim);
}

template <typename MatrixType>
inline MatrixInverter<MatrixType>& MatrixInverter<MatrixType>::compute(const Matrix& A)
{
  // Check preconditions
  assert(A.rows() == svd_.rows() && A.cols() == svd_.cols() &&
         "Size mismatch between MatrixInverter and input matrix.");

  // Compute SVD of input matrix
  svd_.compute(A);
  return *this;
}

template <typename MatrixType>
inline typename MatrixInverter<MatrixType>::Index MatrixInverter<MatrixType>::nonzeroSingularValues() const
{
  const SVD&    svd  = svd_;
  const Vector& S    = svd.singularValues();
  const Index   end  = svd.nonzeroSingularValues(); // Number of singular values that are not _exactly_ zero

  Index id;
  // Loop going from smallest _nonzero_ singular value to first one larger than the prescribed tolerance
  for (id = end; id > 0; --id) { if (S(id -1) >= lsEps_ ) {break;} }
  return id;
}

template <typename MatrixType>
inline typename MatrixInverter<MatrixType>::Scalar
MatrixInverter<MatrixType>::computeDampingSq(const Vector& S) const
{
  const Scalar Smin        = S(S.size() - 1); // Smallest singular value
  return (Smin >= dlsEps_) ? 0.0 : (1.0 - (Smin * Smin) / (dlsEps_ * dlsEps_)) * (dampMax_ * dampMax_);
}

} // KDL


namespace Eigen
{
namespace internal
{

template<typename MatrixType>
struct traits<LsInverseReturnType<MatrixType> > {typedef typename MatrixType::PlainObject ReturnType;};

template<typename MatrixType>
class LsInverseReturnType : public Eigen::ReturnByValue<LsInverseReturnType<MatrixType> >
{
public:
  typedef MatrixType Matrix;
  typedef typename Matrix::Index Index;
  typedef typename Matrix::Scalar Scalar;
  typedef typename KDL::MatrixInverter<MatrixType> MatrixInverter;
  typedef typename MatrixInverter::Vector Vector;
  typedef typename MatrixInverter::SVD SVD;

  LsInverseReturnType(MatrixInverter& inverter) : inverter_(inverter) {}

  template <typename ResultType> void evalTo(ResultType& result) const;

  Index rows() const { return inverter_.svd_.cols(); } 
  Index cols() const { return inverter_.svd_.rows(); } 

private:
  MatrixInverter& inverter_; 
};

template<typename MatrixType>
struct traits<DlsInverseReturnType<MatrixType> > {typedef typename MatrixType::PlainObject ReturnType;};

template<typename MatrixType>
class DlsInverseReturnType : public Eigen::ReturnByValue<DlsInverseReturnType<MatrixType> >
{
public:
  typedef MatrixType Matrix;
  typedef typename Matrix::Index Index;
  typedef typename Matrix::Scalar Scalar;
  typedef typename KDL::MatrixInverter<MatrixType> MatrixInverter;
  typedef typename MatrixInverter::Vector Vector;
  typedef typename MatrixInverter::SVD SVD;

  DlsInverseReturnType(MatrixInverter& inverter) : inverter_(inverter) {}

  template <typename ResultType> void evalTo(ResultType& result) const;

  Index rows() const { return inverter_.svd_.cols(); } 
  Index cols() const { return inverter_.svd_.rows(); } 

private:
  MatrixInverter& inverter_; 
};

template<typename MatrixType>
struct traits<LsSolveReturnType<MatrixType> > {typedef typename MatrixType::PlainObject ReturnType;};

template<typename MatrixType>
class LsSolveReturnType : public Eigen::ReturnByValue<LsSolveReturnType<MatrixType> >
{
public:
  typedef MatrixType Matrix;
  typedef typename Matrix::Index Index;
  typedef typename Matrix::Scalar Scalar;
  typedef typename KDL::MatrixInverter<MatrixType> MatrixInverter;
  typedef typename MatrixInverter::Vector Vector;
  typedef typename MatrixInverter::SVD SVD;

  LsSolveReturnType(const Vector&   b,
                    MatrixInverter& inverter) : b_(b), inverter_(inverter) {}

  template <typename ResultType> void evalTo(ResultType& result) const;

  Index rows() const { return inverter_.svd_.cols(); } 
  Index cols() const { return 1; }

private:
  const Vector&   b_;        
  MatrixInverter& inverter_; 
};

template<typename MatrixType>
struct traits<DlsSolveReturnType<MatrixType> > {typedef typename MatrixType::PlainObject ReturnType;};

template<typename MatrixType>
class DlsSolveReturnType : public Eigen::ReturnByValue<DlsSolveReturnType<MatrixType> >
{
public:
  typedef MatrixType Matrix;
  typedef typename Matrix::Index Index;
  typedef typename Matrix::Scalar Scalar;
  typedef typename KDL::MatrixInverter<MatrixType> MatrixInverter;
  typedef typename MatrixInverter::Vector Vector;
  typedef typename MatrixInverter::SVD SVD;

  DlsSolveReturnType(const Vector&   b,
                     MatrixInverter& inverter) : b_(b), inverter_(inverter) {}

  template <typename ResultType> void evalTo(ResultType& result) const;

  Index rows() const { return inverter_.svd_.cols(); } 
  Index cols() const { return 1; }

private:
  const Vector&   b_;        
  MatrixInverter& inverter_; 
};

template <typename MatrixType> template <typename ResultType>
void LsInverseReturnType<MatrixType>::evalTo(ResultType& result) const
{
  // Work matrix
  Matrix& T = inverter_.pinv_;

  // SVD details
  const SVD&  svd  = inverter_.svd_;
  const Index rows = svd.rows();
  const Index cols = svd.cols();

  const Vector& S = svd.singularValues(); // Sorted in decreasing order
  const Matrix& U = svd.matrixU();
  const Matrix& V = svd.matrixV();

  // Perform V Sinv U' subject to these optimizations:
  // - Space: Use the smallest possible temporaries. This is purpose of the (rows < cols) test
  // - Time:  Exclude unused blocks of U, S, V and work matrices from the computation. They correspond to the
  //   elements that are multiplied by the entries of S cutoff to zero.
  const Index cutoffSize = inverter_.nonzeroSingularValues();
  if (rows < cols)
  {
    T.topLeftCorner(cutoffSize, rows).noalias() =
    S.head(cutoffSize).array().inverse().matrix().asDiagonal() * U.leftCols(cutoffSize).transpose(); // Sinv U'
    result.noalias() = V.leftCols(cutoffSize) * T.topLeftCorner(cutoffSize, rows);                   // V Sinv U'
  }
  else
  {
    T.topLeftCorner(cols, cutoffSize).noalias() =
    V.leftCols(cutoffSize) * S.head(cutoffSize).array().inverse().matrix().asDiagonal();       // V Sinv
    result.noalias() = T.topLeftCorner(cols, cutoffSize) * U.leftCols(cutoffSize).transpose(); // V Sinv U'
  }
}

template <typename MatrixType> template <typename ResultType>
void DlsInverseReturnType<MatrixType>::evalTo(ResultType& result) const
{
  // Work matrix
  Matrix& T = inverter_.pinv_;

  // SVD details
  const SVD&  svd  = inverter_.svd_;
  const Index rows = svd.rows();
  const Index cols = svd.cols();

  const Vector& S = svd.singularValues(); // Sorted in decreasing order
  const Matrix& U = svd.matrixU();
  const Matrix& V = svd.matrixV();

  // Damped least-squares inverse: V Sinv U'
  const Scalar dampSq = inverter_.computeDampingSq(S);
  const Index  minDim = std::min(rows, cols);
  if (rows < cols)
  {
    T.topLeftCorner(minDim, minDim).noalias() =
    (S.array() / (S.array().square() + dampSq)).matrix().asDiagonal() * U.transpose(); // Sinv U'
    result.noalias() = V * T.topLeftCorner(minDim, minDim);                            // V Sinv U'
  }
  else
  {
    T.topLeftCorner(minDim, minDim).noalias() =
    V * (S.array() / (S.array().square() + dampSq)).matrix().asDiagonal();  // V Sinv
    result.noalias() = T.topLeftCorner(minDim, minDim) * U.transpose();     // V Sinv U'
  }
}

template <typename MatrixType> template <typename ResultType>
inline void LsSolveReturnType<MatrixType>::evalTo(ResultType& result) const
{
  // Work vectors
  Vector& vec1 = inverter_.solve1_;
  Vector& vec2 = inverter_.solve2_;

  // SVD details
  const SVD&    svd = inverter_.svd_;
  const Vector& S   = svd.singularValues(); // Sorted in decreasing order
  const Matrix& U   = svd.matrixU();
  const Matrix& V   = svd.matrixV();

  // Perform V Sinv U' b subject to these optimizations:
  // - Space, time: Favor matrix-vector products over matrix-matrix products.
  // - Time:  Exclude unused blocks of U, S, V and work matrices from the computation. They correspond to the
  //   elements that are multiplied by the entries of S cutoff to zero.
  const Index cutoffSize = inverter_.nonzeroSingularValues();

  vec1.head(cutoffSize).noalias() = U.leftCols(cutoffSize).transpose() * b_;                     // U' b
  vec2.head(cutoffSize).noalias() = S.head(cutoffSize).array().inverse().matrix().asDiagonal() *
                                    vec1.head(cutoffSize);                                       // Sinv U' b
  result.noalias()                = V.leftCols(cutoffSize) * vec2.head(cutoffSize);              // V Sinv U' b
}

template <typename MatrixType> template <typename ResultType>
inline void DlsSolveReturnType<MatrixType>::evalTo(ResultType& result) const
{
  // Work vectors
  Vector& vec1 = inverter_.solve1_;
  Vector& vec2 = inverter_.solve2_;

  // SVD details
  const SVD&    svd = inverter_.svd_;
  const Vector& S   = svd.singularValues(); // Sorted in decreasing order
  const Matrix& U   = svd.matrixU();
  const Matrix& V   = svd.matrixV();

  // Perform V Sinv U' b subject to these optimizations:
  // Space, time: Favor matrix-vector products over matrix-matrix products.
  const Scalar dampSq = inverter_.computeDampingSq(S);

  vec1.noalias()   = U.transpose() * b_;                                                       // U' b
  vec2.noalias()   = (S.array() / (S.array().square() + dampSq)).matrix().asDiagonal() * vec1; // Sinv U' b
  result.noalias() = V * vec2;                                                                 // V Sinv U' b
}

} // Eigen
} // internal

#endif // KDL_MATRIXINVERTER_H