GCC Code Coverage Report

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