GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/eigenvalues.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 40 59 67.8%
Branches: 23 84 27.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022-2024 INRIA
3 //
4
5 #ifndef __pinocchio_math_eigenvalues_hpp__
6 #define __pinocchio_math_eigenvalues_hpp__
7
8 #include "pinocchio/math/fwd.hpp"
9 #include <Eigen/Core>
10
11 namespace pinocchio
12 {
13
14 /// \brief Compute the largest eigenvalues and the associated principle eigenvector via power
15 /// iteration
16 template<typename _Vector>
17 struct PowerIterationAlgoTpl
18 {
19 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(_Vector) Vector;
20 typedef typename Vector::Scalar Scalar;
21
22 4489 explicit PowerIterationAlgoTpl(
23 const Eigen::DenseIndex size, const int max_it = 10, const Scalar rel_tol = Scalar(1e-8))
24 4489 : principal_eigen_vector(size)
25
1/2
✓ Branch 1 taken 4489 times.
✗ Branch 2 not taken.
 4489 , lowest_eigen_vector(size)
26 4489 , max_it(max_it)
27 4489 , it(0)
28 4489 , rel_tol(rel_tol)
29
1/2
✓ Branch 1 taken 4489 times.
✗ Branch 2 not taken.
 4489 , eigen_vector_prev(size)
30 {
31
1/2
✓ Branch 1 taken 4489 times.
✗ Branch 2 not taken.
 4489 reset();
32 4489 }
33
34 template<typename MatrixLike>
35 1574 void run(const MatrixLike & mat)
36 {
37
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1574 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1574 PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
38
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1574 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 1574 PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
39
1/2
✓ Branch 1 taken 1574 times.
✗ Branch 2 not taken.
 1574 Scalar eigenvalue_est = principal_eigen_vector.norm();
40
41
2/2
✓ Branch 0 taken 38767 times.
✓ Branch 1 taken 999 times.
 39766 for (it = 0; it < max_it; ++it)
42 {
43 38767 const Scalar eigenvalue_est_prev = eigenvalue_est;
44
1/2
✓ Branch 1 taken 38767 times.
✗ Branch 2 not taken.
 38767 principal_eigen_vector /= eigenvalue_est;
45
1/2
✓ Branch 1 taken 38767 times.
✗ Branch 2 not taken.
 38767 eigen_vector_prev = principal_eigen_vector;
46
3/6
✓ Branch 1 taken 38767 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 38767 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 38767 times.
✗ Branch 8 not taken.
 38767 principal_eigen_vector.noalias() = mat * eigen_vector_prev;
47
48
1/2
✓ Branch 1 taken 38767 times.
✗ Branch 2 not taken.
 38767 eigenvalue_est = principal_eigen_vector.norm();
49
50 38767 convergence_criteria = math::fabs(eigenvalue_est_prev - eigenvalue_est);
51 77534 if (check_expression_if_real<Scalar, false>(
52 38767 convergence_criteria
53
4/6
✓ Branch 3 taken 38767 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 38767 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 575 times.
✓ Branch 9 taken 38192 times.
 38767 <= rel_tol * math::max(math::fabs(eigenvalue_est_prev), math::fabs(eigenvalue_est))))
54 575 break;
55 }
56
57 1574 largest_eigen_value = eigenvalue_est;
58 1574 }
59
60 template<typename MatrixLike, typename VectorLike>
61 void run(const MatrixLike & mat, const Eigen::PlainObjectBase<VectorLike> & eigenvector_est)
62 {
63 principal_eigen_vector = eigenvector_est;
64 run(mat);
65 }
66
67 template<typename MatrixLike>
68 ✗ void lowest(const MatrixLike & mat, const bool compute_largest = true)
69 {
70 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1);
71 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0));
72
73 ✗ if (compute_largest)
74 ✗ run(mat);
75
76 ✗ Scalar eigenvalue_est = lowest_eigen_vector.norm();
77
78 ✗ for (it = 0; it < max_it; ++it)
79 {
80 ✗ const Scalar eigenvalue_est_prev = eigenvalue_est;
81 ✗ lowest_eigen_vector /= eigenvalue_est;
82 ✗ eigen_vector_prev = lowest_eigen_vector;
83 ✗ lowest_eigen_vector.noalias() = mat * eigen_vector_prev;
84 ✗ lowest_eigen_vector -= largest_eigen_value * eigen_vector_prev;
85
86 ✗ eigenvalue_est = lowest_eigen_vector.norm();
87
88 ✗ convergence_criteria = math::fabs(eigenvalue_est_prev - eigenvalue_est);
89 ✗ if (check_expression_if_real<Scalar, false>(
90 ✗ convergence_criteria
91 ✗ <= rel_tol * math::max(math::fabs(eigenvalue_est_prev), math::fabs(eigenvalue_est))))
92 ✗ break;
93 }
94
95 ✗ lowest_eigen_value = largest_eigen_value - eigenvalue_est;
96 }
97
98 template<typename MatrixLike, typename VectorLike>
99 void lowest(
100 const MatrixLike & mat,
101 const Eigen::PlainObjectBase<VectorLike> & largest_eigenvector_est,
102 const Eigen::PlainObjectBase<VectorLike> & lowest_eigenvector_est,
103 const bool compute_largest = true)
104 {
105 principal_eigen_vector = largest_eigenvector_est;
106 lowest_eigen_vector = lowest_eigenvector_est;
107 lowest(mat, compute_largest);
108 }
109
110 4489 void reset()
111 {
112 4489 const Scalar normalized_value = Scalar(1) / math::sqrt(Scalar(principal_eigen_vector.size()));
113
1/2
✓ Branch 1 taken 4489 times.
✗ Branch 2 not taken.
 4489 principal_eigen_vector.fill(normalized_value);
114
1/2
✓ Branch 1 taken 4489 times.
✗ Branch 2 not taken.
 4489 lowest_eigen_vector.fill(normalized_value);
115
116 4489 largest_eigen_value = std::numeric_limits<Scalar>::min();
117 4489 lowest_eigen_value = std::numeric_limits<Scalar>::max();
118 4489 }
119
120 Vector principal_eigen_vector;
121 Vector lowest_eigen_vector;
122 Scalar largest_eigen_value;
123 Scalar lowest_eigen_value;
124 int max_it;
125 int it;
126 Scalar rel_tol;
127 Scalar convergence_criteria;
128
129 protected:
130 Vector eigen_vector_prev;
131 }; // struct PowerIterationAlgoTpl
132
133 ///
134 ///  \brief Compute the lagest eigenvector of a given matrix according to a given eigenvector
135 /// estimate.
136 ///
137 template<typename MatrixLike, typename VectorLike>
138 void computeLargestEigenvector(
139 const MatrixLike & mat,
140 const Eigen::PlainObjectBase<VectorLike> & _eigenvector_est,
141 const int max_it = 10,
142 const typename MatrixLike::Scalar rel_tol = 1e-8)
143 {
144 PowerIterationAlgoTpl<VectorLike> algo(mat.rows(), max_it, rel_tol);
145 algo.run(mat, _eigenvector_est.derived());
146 _eigenvector_est.const_cast_derived() = algo.principal_eigen_vector;
147 }
148
149 ///
150 ///  \brief Compute the lagest eigenvector of a given matrix.
151 ///
152 template<typename MatrixLike>
153 Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1>
154 1574 computeLargestEigenvector(
155 const MatrixLike & mat, const int max_it = 10, const typename MatrixLike::Scalar rel_tol = 1e-8)
156 {
157 typedef Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1> Vector;
158
1/2
✓ Branch 2 taken 1574 times.
✗ Branch 3 not taken.
 1574 PowerIterationAlgoTpl<Vector> algo(mat.rows(), max_it, rel_tol);
159
1/2
✓ Branch 1 taken 1574 times.
✗ Branch 2 not taken.
 1574 algo.run(mat);
160
1/2
✓ Branch 1 taken 1574 times.
✗ Branch 2 not taken.
 3148 return algo.principal_eigen_vector;
161 1574 }
162
163 ///
164 ///  \brief Compute the lagest eigenvector of a given matrix according to a given eigenvector
165 /// estimate.
166 ///
167 template<typename MatrixLike, typename VectorLike1, typename VectorLike2>
168 void computeLowestEigenvector(
169 const MatrixLike & mat,
170 const Eigen::PlainObjectBase<VectorLike1> & largest_eigenvector_est,
171 const Eigen::PlainObjectBase<VectorLike2> & lowest_eigenvector_est,
172 const bool compute_largest = true,
173 const int max_it = 10,
174 const typename MatrixLike::Scalar rel_tol = 1e-8)
175 {
176 PowerIterationAlgoTpl<VectorLike1> algo(mat.rows(), max_it, rel_tol);
177 algo.lowest(
178 mat, largest_eigenvector_est.derived(), lowest_eigenvector_est.derived(), compute_largest);
179 largest_eigenvector_est.const_cast_derived() = algo.principal_eigen_vector;
180 lowest_eigenvector_est.const_cast_derived() = algo.lowest_eigen_vector;
181 }
182
183 ///
184 ///  \brief Compute the lagest eigenvector of a given matrix.
185 ///
186 template<typename MatrixLike>
187 Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1>
188 computeLowestEigenvector(
189 const MatrixLike & mat,
190 const bool compute_largest = true,
191 const int max_it = 10,
192 const typename MatrixLike::Scalar rel_tol = 1e-8)
193 {
194 typedef Eigen::Matrix<typename MatrixLike::Scalar, MatrixLike::RowsAtCompileTime, 1> Vector;
195 PowerIterationAlgoTpl<Vector> algo(mat.rows(), max_it, rel_tol);
196 algo.lowest(mat, compute_largest);
197 return algo.lowest_eigen_vector;
198 }
199
200 ///
201 ///  \brief Compute the lagest eigenvalue of a given matrix. This is taking the eigenvector
202 /// computed by the function computeLargestEigenvector.
203 ///
204 template<typename VectorLike>
205 typename VectorLike::Scalar
206 1574 retrieveLargestEigenvalue(const Eigen::MatrixBase<VectorLike> & eigenvector)
207 {
208 1574 return eigenvector.norm();
209 }
210 } // namespace pinocchio
211
212 #endif // #ifndef __pinocchio_math_eigenvalues_hpp__
213