Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/utils/math.hpp	Lines:	11	12	91.7 %
Date:	2024-02-13 11:12:33	Branches:	21	42	50.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
#define CROCODDYL_CORE_UTILS_MATH_HPP_

#include <Eigen/Dense>
#include <algorithm>
#include <boost/type_traits.hpp>
#include <limits>

// fwd

template <typename MatrixLike,
          bool value =
              boost::is_floating_point<typename MatrixLike::Scalar>::value>
struct pseudoInverseAlgo {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
                        const RealScalar& epsilon) {
    using std::max;
    Eigen::JacobiSVD<MatrixLike> svd(a,
                                     Eigen::ComputeThinU | Eigen::ComputeThinV);
    RealScalar tolerance = epsilon *
                           static_cast<Scalar>(max(a.cols(), a.rows())) *
                           svd.singularValues().array().abs()(0);
    return svd.matrixV() *
           (svd.singularValues().array().abs() > tolerance)
               .select(svd.singularValues().array().inverse(), 0)
               .matrix()
               .asDiagonal() *
           svd.matrixU().adjoint();
  }
};

template <typename MatrixLike>
struct pseudoInverseAlgo<MatrixLike, false> {
  typedef typename MatrixLike::Scalar Scalar;
  typedef typename MatrixLike::RealScalar RealScalar;

  static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
                        const RealScalar&) {
    return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
  }
};

template <typename MatrixLike>
MatrixLike pseudoInverse(
    const Eigen::MatrixBase<MatrixLike>& a,
    const typename MatrixLike::RealScalar& epsilon =
        Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
  return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
}

#endif  // CROCODDYL_CORE_UTILS_MATH_HPP_


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019, LAAS-CNRS
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_CORE_UTILS_MATH_HPP_
10			#define CROCODDYL_CORE_UTILS_MATH_HPP_
11
12			#include <Eigen/Dense>
13			#include <algorithm>
14			#include <boost/type_traits.hpp>
15			#include <limits>
16
17			// fwd
18
19			template <typename MatrixLike,
20			bool value =
21			boost::is_floating_point<typename MatrixLike::Scalar>::value>
22			struct pseudoInverseAlgo {
23			typedef typename MatrixLike::Scalar Scalar;
24			typedef typename MatrixLike::RealScalar RealScalar;
25
26		10326	static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
27			const RealScalar& epsilon) {
28			using std::max;
29	✓✗✓✗	10326	Eigen::JacobiSVD<MatrixLike> svd(a,
30			Eigen::ComputeThinU \| Eigen::ComputeThinV);
31		30978	RealScalar tolerance = epsilon *
32	✓✗✓✗	10326	static_cast<Scalar>(max(a.cols(), a.rows())) *
33	✓✗✓✗ ✓✗	10326	svd.singularValues().array().abs()(0);
34	✓✗	10326	return svd.matrixV() *
35	✓✗	10326	(svd.singularValues().array().abs() > tolerance)
36	✓✗	10326	.select(svd.singularValues().array().inverse(), 0)
37			.matrix()
38			.asDiagonal() *
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	51630	svd.matrixU().adjoint();
40			}
41			};
42
43			template <typename MatrixLike>
44			struct pseudoInverseAlgo<MatrixLike, false> {
45			typedef typename MatrixLike::Scalar Scalar;
46			typedef typename MatrixLike::RealScalar RealScalar;
47
48			static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
49			const RealScalar&) {
50			return Eigen::MatrixBase<MatrixLike>::Zero(a.rows(), a.cols());
51			}
52			};
53
54			template <typename MatrixLike>
55		10326	MatrixLike pseudoInverse(
56			const Eigen::MatrixBase<MatrixLike>& a,
57			const typename MatrixLike::RealScalar& epsilon =
58			Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
59		10326	return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
60			}
61
62			#endif // CROCODDYL_CORE_UTILS_MATH_HPP_