Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/cholesky.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 135 | 176 | 76.7% |
Branches: | 109 | 473 | 23.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_cholesky_hxx__ | ||
6 | #define __pinocchio_cholesky_hxx__ | ||
7 | |||
8 | #include "pinocchio/algorithm/check.hpp" | ||
9 | |||
10 | /// @cond DEV | ||
11 | |||
12 | namespace pinocchio | ||
13 | { | ||
14 | namespace cholesky | ||
15 | { | ||
16 | |||
17 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
18 | 225 | inline const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & decompose( | |
19 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
20 | DataTpl<Scalar, Options, JointCollectionTpl> & data) | ||
21 | { | ||
22 | /* | ||
23 | * D = zeros(n,1); | ||
24 | * U = eye(n); | ||
25 | * for j=n:-1:1 | ||
26 | * subtree = j+1:tree(j); | ||
27 | * D(j) = M(j,j) - U(j,subtree)*diag(D(subtree))*U(j,subtree)'; | ||
28 | * i=parent(j); | ||
29 | * while i>0 | ||
30 | * U(i,j) = (M(i,j) - U(i,subtree)*diag(D(subtree))*U(j,subtree)') / D(j); | ||
31 | * i=parent(i); | ||
32 | * end | ||
33 | * end | ||
34 | */ | ||
35 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
36 |
1/2✓ Branch 1 taken 225 times.
✗ Branch 2 not taken.
|
225 | assert(model.check(data) && "data is not consistent with model."); |
37 | |||
38 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
39 | |||
40 | 225 | const typename Data::MatrixXs & M = data.M; | |
41 | 225 | typename Data::MatrixXs & U = data.U; | |
42 | 225 | typename Data::VectorXs & D = data.D; | |
43 | 225 | typename Data::VectorXs & Dinv = data.Dinv; | |
44 | |||
45 |
2/2✓ Branch 0 taken 7186 times.
✓ Branch 1 taken 225 times.
|
7411 | for (int j = model.nv - 1; j >= 0; --j) |
46 | { | ||
47 | 7186 | const int NVT = data.nvSubtree_fromRow[(size_t)j] - 1; | |
48 |
1/2✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
|
7186 | typename Data::VectorXs::SegmentReturnType DUt = data.tmp.head(NVT); |
49 |
2/2✓ Branch 0 taken 6286 times.
✓ Branch 1 taken 900 times.
|
7186 | if (NVT) |
50 |
3/6✓ Branch 1 taken 6286 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6286 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6286 times.
✗ Branch 8 not taken.
|
6286 | DUt.noalias() = |
51 |
4/8✓ Branch 1 taken 6286 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6286 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6286 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6286 times.
✗ Branch 11 not taken.
|
12572 | U.row(j).segment(j + 1, NVT).transpose().cwiseProduct(D.segment(j + 1, NVT)); |
52 | |||
53 |
5/10✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7186 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7186 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7186 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7186 times.
✗ Branch 14 not taken.
|
7186 | D[j] = M(j, j) - U.row(j).segment(j + 1, NVT).dot(DUt); |
54 |
2/4✓ Branch 1 taken 7186 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7186 times.
✗ Branch 5 not taken.
|
7186 | Dinv[j] = Scalar(1) / D[j]; |
55 |
2/2✓ Branch 1 taken 57443 times.
✓ Branch 2 taken 7186 times.
|
64629 | for (int _i = data.parents_fromRow[(size_t)j]; _i >= 0; |
56 | 57443 | _i = data.parents_fromRow[(size_t)_i]) | |
57 |
6/12✓ Branch 1 taken 57443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 57443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 57443 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 57443 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 57443 times.
✗ Branch 17 not taken.
|
57443 | U(_i, j) = (M(_i, j) - U.row(_i).segment(j + 1, NVT).dot(DUt)) * Dinv[j]; |
58 | } | ||
59 | |||
60 | 225 | return data.U; | |
61 | } | ||
62 | |||
63 | namespace internal | ||
64 | { | ||
65 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
66 | struct Uv | ||
67 | { | ||
68 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
69 | 1 | static void run( | |
70 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
71 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
72 | const Eigen::MatrixBase<Mat> & m) | ||
73 | { | ||
74 | 1 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
75 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
|
21 | for (int k = 0; k < m_.cols(); ++k) |
76 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | cholesky::Uv(model, data, m_.col(k)); |
77 | 1 | } | |
78 | }; | ||
79 | |||
80 | template<typename Mat> | ||
81 | struct Uv<Mat, 1> | ||
82 | { | ||
83 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
84 | 20 | static void run( | |
85 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
86 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
87 | const Eigen::MatrixBase<Mat> & v) | ||
88 | { | ||
89 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
90 | |||
91 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
92 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
93 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | assert(model.check(data) && "data is not consistent with model."); |
94 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
20 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
95 | |||
96 | 20 | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
97 | |||
98 | 20 | const typename Data::MatrixXs & U = data.U; | |
99 | 20 | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
100 | |||
101 |
2/2✓ Branch 0 taken 620 times.
✓ Branch 1 taken 20 times.
|
640 | for (int k = 0; k < model.nv - 1; ++k) // You can stop one step before nv |
102 |
4/8✓ Branch 1 taken 620 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 620 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 620 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 620 times.
✗ Branch 12 not taken.
|
1240 | v_.row(k) += U.row(k).segment(k + 1, nvt[(size_t)k] - 1) |
103 |
1/2✓ Branch 3 taken 620 times.
✗ Branch 4 not taken.
|
1860 | * v_.middleRows(k + 1, nvt[(size_t)k] - 1); |
104 | 20 | } | |
105 | }; | ||
106 | |||
107 | } // namespace internal | ||
108 | |||
109 | /* Compute U*v. | ||
110 | * Nota: there is no smart way of doing U*D*v, so it is not proposed. */ | ||
111 | template< | ||
112 | typename Scalar, | ||
113 | int Options, | ||
114 | template<typename, int> | ||
115 | class JointCollectionTpl, | ||
116 | typename Mat> | ||
117 | 42 | Mat & Uv( | |
118 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
119 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
120 | const Eigen::MatrixBase<Mat> & m) | ||
121 | { | ||
122 | 42 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
123 | 42 | internal::Uv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_); | |
124 | 42 | return m_.derived(); | |
125 | } | ||
126 | |||
127 | namespace internal | ||
128 | { | ||
129 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
130 | struct Utv | ||
131 | { | ||
132 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
133 | 1 | static void run( | |
134 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
135 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
136 | const Eigen::MatrixBase<Mat> & m) | ||
137 | { | ||
138 | 1 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
139 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
|
21 | for (int k = 0; k < m_.cols(); ++k) |
140 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | cholesky::Utv(model, data, m_.col(k)); |
141 | 1 | } | |
142 | }; | ||
143 | |||
144 | template<typename Mat> | ||
145 | struct Utv<Mat, 1> | ||
146 | { | ||
147 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
148 | 20 | static void run( | |
149 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
150 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
151 | const Eigen::MatrixBase<Mat> & v) | ||
152 | { | ||
153 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
154 | |||
155 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
156 | |||
157 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
158 |
1/2✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
|
20 | assert(model.check(data) && "data is not consistent with model."); |
159 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
20 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
160 | 20 | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
161 | |||
162 | 20 | const typename Data::MatrixXs & U = data.U; | |
163 | 20 | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
164 | |||
165 |
2/2✓ Branch 0 taken 620 times.
✓ Branch 1 taken 20 times.
|
640 | for (int k = model.nv - 2; k >= 0; --k) // You can start from nv-2 (no child in nv-1) |
166 |
3/6✓ Branch 1 taken 620 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 620 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 620 times.
✗ Branch 9 not taken.
|
620 | v_.segment(k + 1, nvt[(size_t)k] - 1) += |
167 |
2/4✓ Branch 4 taken 620 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 620 times.
✗ Branch 8 not taken.
|
1240 | U.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * v_[k]; |
168 | 20 | } | |
169 | }; | ||
170 | |||
171 | } // namespace internal | ||
172 | |||
173 | /* Compute U'*v */ | ||
174 | template< | ||
175 | typename Scalar, | ||
176 | int Options, | ||
177 | template<typename, int> | ||
178 | class JointCollectionTpl, | ||
179 | typename Mat> | ||
180 | 42 | Mat & Utv( | |
181 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
182 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
183 | const Eigen::MatrixBase<Mat> & m) | ||
184 | { | ||
185 | 42 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
186 | 42 | internal::Utv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_); | |
187 | 42 | return m_.derived(); | |
188 | } | ||
189 | |||
190 | namespace internal | ||
191 | { | ||
192 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
193 | struct Uiv | ||
194 | { | ||
195 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
196 | 217 | static void run( | |
197 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
198 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
199 | const Eigen::MatrixBase<Mat> & m) | ||
200 | { | ||
201 | 217 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
202 |
2/2✓ Branch 1 taken 1439 times.
✓ Branch 2 taken 217 times.
|
1656 | for (int k = 0; k < m_.cols(); ++k) |
203 |
1/2✓ Branch 2 taken 1439 times.
✗ Branch 3 not taken.
|
1439 | cholesky::Uiv(model, data, m_.col(k)); |
204 | 217 | } | |
205 | }; | ||
206 | |||
207 | template<typename Mat> | ||
208 | struct Uiv<Mat, 1> | ||
209 | { | ||
210 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
211 | 1462 | static void run( | |
212 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
213 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
214 | const Eigen::MatrixBase<Mat> & v) | ||
215 | { | ||
216 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
217 | |||
218 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
219 |
1/2✓ Branch 1 taken 1460 times.
✗ Branch 2 not taken.
|
1462 | assert(model.check(data) && "data is not consistent with model."); |
220 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 1460 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
1462 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
221 | 1462 | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
222 | |||
223 | 1462 | const typename Data::MatrixXs & U = data.U; | |
224 | 1462 | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
225 | |||
226 |
2/2✓ Branch 0 taken 45064 times.
✓ Branch 1 taken 1460 times.
|
46560 | for (int k = model.nv - 2; k >= 0; --k) // You can start from nv-2 (no child in nv-1) |
227 | 45098 | v_[k] -= U.row(k) | |
228 |
1/2✓ Branch 2 taken 45064 times.
✗ Branch 3 not taken.
|
45098 | .segment(k + 1, nvt[(size_t)k] - 1) |
229 |
3/6✓ Branch 2 taken 45064 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45064 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45064 times.
✗ Branch 9 not taken.
|
45098 | .dot(v_.segment(k + 1, nvt[(size_t)k] - 1)); |
230 | 1462 | } | |
231 | }; | ||
232 | |||
233 | } // namespace internal | ||
234 | |||
235 | /* Compute U^{-1}*v | ||
236 | * Nota: there is no efficient way to compute D^{-1}U^{-1}v | ||
237 | * in a single loop, so algorithm is not proposed.*/ | ||
238 | template< | ||
239 | typename Scalar, | ||
240 | int Options, | ||
241 | template<typename, int> | ||
242 | class JointCollectionTpl, | ||
243 | typename Mat> | ||
244 | 3335 | Mat & Uiv( | |
245 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
246 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
247 | const Eigen::MatrixBase<Mat> & m) | ||
248 | { | ||
249 | 3335 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
250 | 3335 | internal::Uiv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_); | |
251 | 3335 | return m_.derived(); | |
252 | } | ||
253 | |||
254 | namespace internal | ||
255 | { | ||
256 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
257 | struct Utiv | ||
258 | { | ||
259 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
260 | 1 | static void run( | |
261 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
262 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
263 | const Eigen::MatrixBase<Mat> & m) | ||
264 | { | ||
265 | 1 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
266 |
2/2✓ Branch 1 taken 20 times.
✓ Branch 2 taken 1 times.
|
21 | for (int k = 0; k < m_.cols(); ++k) |
267 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | cholesky::Utiv(model, data, m_.col(k)); |
268 | 1 | } | |
269 | }; | ||
270 | |||
271 | template<typename Mat> | ||
272 | struct Utiv<Mat, 1> | ||
273 | { | ||
274 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
275 | 43 | static void run( | |
276 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
277 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
278 | const Eigen::MatrixBase<Mat> & v) | ||
279 | { | ||
280 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
281 | |||
282 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
283 |
1/2✓ Branch 1 taken 41 times.
✗ Branch 2 not taken.
|
43 | assert(model.check(data) && "data is not consistent with model."); |
284 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 41 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
43 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
285 | 43 | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
286 | |||
287 | 43 | const typename Data::MatrixXs & U = data.U; | |
288 | 43 | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
289 | |||
290 |
2/2✓ Branch 0 taken 1243 times.
✓ Branch 1 taken 41 times.
|
1320 | for (int k = 0; k < model.nv - 1; ++k) // You can stop one step before nv. |
291 |
3/6✓ Branch 1 taken 1243 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1243 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1243 times.
✗ Branch 9 not taken.
|
1277 | v_.segment(k + 1, nvt[(size_t)k] - 1) -= |
292 |
2/4✓ Branch 4 taken 1243 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1243 times.
✗ Branch 8 not taken.
|
2554 | U.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * v_[k]; |
293 | 43 | } | |
294 | }; | ||
295 | |||
296 | } // namespace internal | ||
297 | |||
298 | template< | ||
299 | typename Scalar, | ||
300 | int Options, | ||
301 | template<typename, int> | ||
302 | class JointCollectionTpl, | ||
303 | typename Mat> | ||
304 | 65 | Mat & Utiv( | |
305 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
306 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
307 | const Eigen::MatrixBase<Mat> & m) | ||
308 | { | ||
309 | 65 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
310 | 65 | internal::Utiv<Mat, Mat::ColsAtCompileTime>::run(model, data, m_); | |
311 | 65 | return m_.derived(); | |
312 | } | ||
313 | |||
314 | namespace internal | ||
315 | { | ||
316 | template<typename Mat, typename MatRes, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
317 | struct Mv | ||
318 | { | ||
319 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
320 | ✗ | static void run( | |
321 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
322 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
323 | const Eigen::MatrixBase<Mat> & min, | ||
324 | const Eigen::MatrixBase<MatRes> & mout) | ||
325 | { | ||
326 | ✗ | MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, mout); | |
327 | ✗ | for (int k = 0; k < min.cols(); ++k) | |
328 | ✗ | cholesky::Mv(model, data, min.col(k), mout_.col(k)); | |
329 | } | ||
330 | }; | ||
331 | |||
332 | template<typename Mat, typename MatRes> | ||
333 | struct Mv<Mat, MatRes, 1> | ||
334 | { | ||
335 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
336 | ✗ | static void run( | |
337 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
338 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
339 | const Eigen::MatrixBase<Mat> & vin, | ||
340 | const Eigen::MatrixBase<MatRes> & vout) | ||
341 | { | ||
342 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
343 | |||
344 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
345 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes) | ||
346 | |||
347 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
348 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
349 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv); | |
350 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv); | |
351 | |||
352 | ✗ | MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, vout); | |
353 | |||
354 | ✗ | const typename Data::MatrixXs & M = data.M; | |
355 | ✗ | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
356 | |||
357 | ✗ | for (int k = model.nv - 1; k >= 0; --k) | |
358 | { | ||
359 | ✗ | vout_[k] = M.row(k).segment(k, nvt[(size_t)k]) * vin.segment(k, nvt[(size_t)k]); | |
360 | ✗ | vout_.segment(k + 1, nvt[(size_t)k] - 1) += | |
361 | ✗ | M.row(k).segment(k + 1, nvt[(size_t)k] - 1).transpose() * vin[k]; | |
362 | } | ||
363 | } | ||
364 | }; | ||
365 | |||
366 | } // namespace internal | ||
367 | |||
368 | template< | ||
369 | typename Scalar, | ||
370 | int Options, | ||
371 | template<typename, int> | ||
372 | class JointCollectionTpl, | ||
373 | typename Mat, | ||
374 | typename MatRes> | ||
375 | ✗ | MatRes & Mv( | |
376 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
377 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
378 | const Eigen::MatrixBase<Mat> & min, | ||
379 | const Eigen::MatrixBase<MatRes> & mout) | ||
380 | { | ||
381 | ✗ | MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes, mout); | |
382 | ✗ | internal::Mv<Mat, MatRes, Mat::ColsAtCompileTime>::run(model, data, min.derived(), mout_); | |
383 | ✗ | return mout_.derived(); | |
384 | } | ||
385 | |||
386 | template< | ||
387 | typename Scalar, | ||
388 | int Options, | ||
389 | template<typename, int> | ||
390 | class JointCollectionTpl, | ||
391 | typename Mat> | ||
392 | ✗ | typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv( | |
393 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
394 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
395 | const Eigen::MatrixBase<Mat> & min) | ||
396 | { | ||
397 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType; | ||
398 | ✗ | ReturnType res(model.nv, min.cols()); | |
399 | ✗ | return Mv(model, data, min, res); | |
400 | } | ||
401 | |||
402 | namespace internal | ||
403 | { | ||
404 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
405 | struct UDUtv | ||
406 | { | ||
407 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
408 | ✗ | static void run( | |
409 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
410 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
411 | const Eigen::MatrixBase<Mat> & m) | ||
412 | { | ||
413 | ✗ | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
414 | ✗ | for (int k = 0; k < m_.cols(); ++k) | |
415 | ✗ | cholesky::UDUtv(model, data, m_.col(k)); | |
416 | } | ||
417 | }; | ||
418 | |||
419 | template<typename Mat> | ||
420 | struct UDUtv<Mat, 1> | ||
421 | { | ||
422 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
423 | ✗ | static void run( | |
424 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
425 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
426 | const Eigen::MatrixBase<Mat> & v) | ||
427 | { | ||
428 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
429 | |||
430 | ✗ | assert(model.check(data) && "data is not consistent with model."); | |
431 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); | |
432 | |||
433 | ✗ | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
434 | |||
435 | ✗ | cholesky::Utv(model, data, v_); | |
436 | ✗ | v_.array() *= data.D.array(); | |
437 | ✗ | cholesky::Uv(model, data, v_); | |
438 | } | ||
439 | }; | ||
440 | |||
441 | } // namespace internal | ||
442 | |||
443 | template< | ||
444 | typename Scalar, | ||
445 | int Options, | ||
446 | template<typename, int> | ||
447 | class JointCollectionTpl, | ||
448 | typename Mat> | ||
449 | ✗ | Mat & UDUtv( | |
450 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
451 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
452 | const Eigen::MatrixBase<Mat> & m) | ||
453 | { | ||
454 | ✗ | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
455 | |||
456 | ✗ | internal::UDUtv<Mat>::run(model, data, m_); | |
457 | ✗ | return m_; | |
458 | } | ||
459 | |||
460 | namespace internal | ||
461 | { | ||
462 | template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> | ||
463 | struct solve | ||
464 | { | ||
465 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
466 | ✗ | static void run( | |
467 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
468 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
469 | const Eigen::MatrixBase<Mat> & m) | ||
470 | { | ||
471 | ✗ | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
472 | ✗ | for (int k = 0; k < m_.cols(); ++k) | |
473 | ✗ | cholesky::solve(model, data, m_.col(k)); | |
474 | } | ||
475 | }; | ||
476 | |||
477 | template<typename Mat> | ||
478 | struct solve<Mat, 1> | ||
479 | { | ||
480 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
481 | 23 | static void run( | |
482 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
483 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
484 | const Eigen::MatrixBase<Mat> & v) | ||
485 | { | ||
486 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) | ||
487 | |||
488 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
23 | assert(model.check(data) && "data is not consistent with model."); |
489 | |||
490 | 23 | Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); | |
491 | |||
492 | 23 | cholesky::Uiv(model, data, v_); | |
493 |
2/4✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
|
23 | v_.array() *= data.Dinv.array(); |
494 | 23 | cholesky::Utiv(model, data, v_); | |
495 | 23 | } | |
496 | }; | ||
497 | |||
498 | } // namespace internal | ||
499 | |||
500 | template< | ||
501 | typename Scalar, | ||
502 | int Options, | ||
503 | template<typename, int> | ||
504 | class JointCollectionTpl, | ||
505 | typename Mat> | ||
506 | 21 | Mat & solve( | |
507 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
508 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
509 | const Eigen::MatrixBase<Mat> & m) | ||
510 | { | ||
511 | 21 | Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, m); | |
512 | 21 | internal::solve<Mat, Mat::ColsAtCompileTime>::run(model, data, m_); | |
513 | 21 | return m_; | |
514 | } | ||
515 | |||
516 | namespace internal | ||
517 | { | ||
518 | template< | ||
519 | typename Scalar, | ||
520 | int Options, | ||
521 | template<typename, int> | ||
522 | class JointCollectionTpl, | ||
523 | typename VectorLike> | ||
524 | 64 | VectorLike & Miunit( | |
525 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
526 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
527 | const int col, | ||
528 | const Eigen::MatrixBase<VectorLike> & v) | ||
529 | { | ||
530 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike); | ||
531 | |||
532 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
533 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
64 | assert(model.check(data) && "data is not consistent with model."); |
534 |
2/6✓ Branch 0 taken 64 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 64 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
64 | PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0); |
535 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 64 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
64 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
536 | |||
537 | typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; | ||
538 | |||
539 | 64 | const typename Data::MatrixXs & U = data.U; | |
540 | 64 | const std::vector<int> & nvt = data.nvSubtree_fromRow; | |
541 | 64 | VectorLike & v_ = PINOCCHIO_EIGEN_CONST_CAST(VectorLike, v); | |
542 | |||
543 | 64 | v_[col] = (typename VectorLike::Scalar)1; | |
544 | 64 | const int last_col = | |
545 | 64 | std::min<int>(col - 1, model.nv - 2); // You can start from nv-2 (no child in nv-1) | |
546 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | v_.tail(model.nv - col - 1).setZero(); |
547 |
2/2✓ Branch 0 taken 992 times.
✓ Branch 1 taken 64 times.
|
1056 | for (int k = last_col; k >= 0; --k) |
548 | { | ||
549 | 992 | const int nvt_max = std::min<int>(col, nvt[(size_t)k] - 1); | |
550 |
4/8✓ Branch 2 taken 992 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 992 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 992 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 992 times.
✗ Branch 12 not taken.
|
992 | v_[k] = -U.row(k).segment(k + 1, nvt_max).dot(v_.segment(k + 1, nvt_max)); |
551 | } | ||
552 | |||
553 |
4/8✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 64 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 64 times.
✗ Branch 12 not taken.
|
64 | v_.head(col + 1).array() *= data.Dinv.head(col + 1).array(); |
554 | |||
555 |
2/2✓ Branch 0 taken 1056 times.
✓ Branch 1 taken 64 times.
|
1120 | for (int k = 0; k < col + 1; ++k) |
556 | { | ||
557 | 1056 | const int nvt_max = nvt[(size_t)k] - 1; | |
558 |
5/10✓ Branch 3 taken 1056 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1056 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1056 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1056 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1056 times.
✗ Branch 16 not taken.
|
1056 | v_.segment(k + 1, nvt_max) -= U.row(k).segment(k + 1, nvt_max).transpose() * v_[k]; |
559 | } | ||
560 | |||
561 | 64 | return v_; | |
562 | } | ||
563 | } // namespace internal | ||
564 | |||
565 | template< | ||
566 | typename Scalar, | ||
567 | int Options, | ||
568 | template<typename, int> | ||
569 | class JointCollectionTpl, | ||
570 | typename Mat> | ||
571 | 2 | Mat & computeMinv( | |
572 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
573 | const DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
574 | const Eigen::MatrixBase<Mat> & Minv) | ||
575 | { | ||
576 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv); |
577 |
1/24✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
|
2 | PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv); |
578 | |||
579 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | assert(model.check(data) && "data is not consistent with model."); |
580 | |||
581 | 2 | Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, Minv); | |
582 | |||
583 |
2/2✓ Branch 0 taken 64 times.
✓ Branch 1 taken 2 times.
|
66 | for (int col = 0; col < model.nv; ++col) |
584 |
1/2✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
|
64 | internal::Miunit(model, data, col, Minv_.col(col)); |
585 | |||
586 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Minv_.template triangularView<Eigen::StrictlyLower>() = |
587 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | Minv_.transpose().template triangularView<Eigen::StrictlyLower>(); |
588 | |||
589 | 2 | return Minv_; | |
590 | } | ||
591 | |||
592 | } // namespace cholesky | ||
593 | } // namespace pinocchio | ||
594 | |||
595 | /// @endcond | ||
596 | |||
597 | #endif // ifndef __pinocchio_cholesky_hxx__ | ||
598 |