Directory: | ./ |
---|---|
File: | include/pinocchio/math/eigenvalues.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 16 | 57 | 28.1% |
Branches: | 5 | 84 | 6.0% |
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 | 2915 | explicit PowerIterationAlgoTpl( | |
23 | const Eigen::DenseIndex size, const int max_it = 10, const Scalar rel_tol = Scalar(1e-8)) | ||
24 | 2915 | : principal_eigen_vector(size) | |
25 |
1/2✓ Branch 1 taken 2915 times.
✗ Branch 2 not taken.
|
2915 | , lowest_eigen_vector(size) |
26 | 2915 | , max_it(max_it) | |
27 | 2915 | , it(0) | |
28 | 2915 | , rel_tol(rel_tol) | |
29 |
1/2✓ Branch 1 taken 2915 times.
✗ Branch 2 not taken.
|
2915 | , eigen_vector_prev(size) |
30 | { | ||
31 |
1/2✓ Branch 1 taken 2915 times.
✗ Branch 2 not taken.
|
2915 | reset(); |
32 | 2915 | } | |
33 | |||
34 | template<typename MatrixLike> | ||
35 | ✗ | void run(const MatrixLike & mat) | |
36 | { | ||
37 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(max_it >= 1); | |
38 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT(rel_tol > Scalar(0)); | |
39 | ✗ | Scalar eigenvalue_est = principal_eigen_vector.norm(); | |
40 | |||
41 | ✗ | for (it = 0; it < max_it; ++it) | |
42 | { | ||
43 | ✗ | const Scalar eigenvalue_est_prev = eigenvalue_est; | |
44 | ✗ | principal_eigen_vector /= eigenvalue_est; | |
45 | ✗ | eigen_vector_prev = principal_eigen_vector; | |
46 | ✗ | principal_eigen_vector.noalias() = mat * eigen_vector_prev; | |
47 | |||
48 | ✗ | eigenvalue_est = principal_eigen_vector.norm(); | |
49 | |||
50 | ✗ | convergence_criteria = math::fabs(eigenvalue_est_prev - eigenvalue_est); | |
51 | ✗ | if (check_expression_if_real<Scalar, false>( | |
52 | ✗ | convergence_criteria | |
53 | ✗ | <= rel_tol * math::max(math::fabs(eigenvalue_est_prev), math::fabs(eigenvalue_est)))) | |
54 | ✗ | break; | |
55 | } | ||
56 | |||
57 | ✗ | largest_eigen_value = eigenvalue_est; | |
58 | } | ||
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 | 2915 | void reset() | |
111 | { | ||
112 | 2915 | const Scalar normalized_value = Scalar(1) / math::sqrt(Scalar(principal_eigen_vector.size())); | |
113 |
1/2✓ Branch 1 taken 2915 times.
✗ Branch 2 not taken.
|
2915 | principal_eigen_vector.fill(normalized_value); |
114 |
1/2✓ Branch 1 taken 2915 times.
✗ Branch 2 not taken.
|
2915 | lowest_eigen_vector.fill(normalized_value); |
115 | |||
116 | 2915 | largest_eigen_value = std::numeric_limits<Scalar>::min(); | |
117 | 2915 | lowest_eigen_value = std::numeric_limits<Scalar>::max(); | |
118 | 2915 | } | |
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 | ✗ | 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 | ✗ | PowerIterationAlgoTpl<Vector> algo(mat.rows(), max_it, rel_tol); | |
159 | ✗ | algo.run(mat); | |
160 | ✗ | return algo.principal_eigen_vector; | |
161 | } | ||
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 | ✗ | retrieveLargestEigenvalue(const Eigen::MatrixBase<VectorLike> & eigenvector) | |
207 | { | ||
208 | ✗ | return eigenvector.norm(); | |
209 | } | ||
210 | } // namespace pinocchio | ||
211 | |||
212 | #endif // #ifndef __pinocchio_math_eigenvalues_hpp__ | ||
213 |