9#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
10#define CROCODDYL_CORE_UTILS_MATH_HPP_
12#include <boost/type_traits.hpp>
20template <typename MatrixLike, bool value = boost::is_floating_point<typename MatrixLike::Scalar>::value>
22 typedef typename MatrixLike::Scalar Scalar;
23 typedef typename MatrixLike::RealScalar RealScalar;
25 static MatrixLike run(
const Eigen::MatrixBase<MatrixLike>& a,
const RealScalar& epsilon) {
27 Eigen::JacobiSVD<MatrixLike> svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);
28 RealScalar tolerance =
29 epsilon *
static_cast<Scalar
>(max(a.cols(), a.rows())) * svd.singularValues().array().abs()(0);
30 return svd.matrixV() *
31 (svd.singularValues().array().abs() > tolerance)
32 .select(svd.singularValues().array().inverse(), 0)
35 svd.matrixU().adjoint();
39template <
typename MatrixLike>
41 typedef typename MatrixLike::Scalar Scalar;
42 typedef typename MatrixLike::RealScalar RealScalar;
44 static MatrixLike run(
const Eigen::MatrixBase<MatrixLike>& a,
const RealScalar&) {
45 return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
49template <
typename MatrixLike>
50MatrixLike pseudoInverse(
const Eigen::MatrixBase<MatrixLike>& a,
51 const typename MatrixLike::RealScalar& epsilon =
52 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {