GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/utils/math.hpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 12 12 100.0%
Branches: 22 44 50.0%
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 11308 static MatrixLike run(const Eigen::MatrixBase<MatrixLike>& a,
27 const RealScalar& epsilon) {
28 using std::max;
29
2/4
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11308 times.
✗ Branch 5 not taken.
 11308 Eigen::JacobiSVD<MatrixLike> svd(a,
30 Eigen::ComputeThinU | Eigen::ComputeThinV);
31 33924 RealScalar tolerance = epsilon *
32 11308 static_cast<Scalar>(max(a.cols(), a.rows())) *
33
4/8
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11308 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11308 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11308 times.
✗ Branch 11 not taken.
 11308 svd.singularValues().array().abs()(0);
34
1/2
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
 11308 return svd.matrixV() *
35
2/4
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11308 times.
✗ Branch 5 not taken.
 11308 (svd.singularValues().array().abs() > tolerance)
36
1/2
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
 11308 .select(svd.singularValues().array().inverse(), 0)
37 .matrix()
38 .asDiagonal() *
39
12/24
✓ Branch 1 taken 11308 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11308 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11308 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11308 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 11308 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 11308 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 11308 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 11308 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 11308 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 11308 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 11308 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 11308 times.
✗ Branch 35 not taken.
 56540 svd.matrixU().adjoint();
40 11308 }
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 11308 MatrixLike pseudoInverse(
56 const Eigen::MatrixBase<MatrixLike>& a,
57 const typename MatrixLike::RealScalar& epsilon =
58 Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision()) {
59 11308 return pseudoInverseAlgo<MatrixLike>::run(a, epsilon);
60 }
61
62 #endif // CROCODDYL_CORE_UTILS_MATH_HPP_
63