GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 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 |
inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs & |
||
19 |
206 |
decompose(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 |
✓✗ | 206 |
assert(model.check(data) && "data is not consistent with model."); |
37 |
|||
38 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
39 |
|||
40 |
206 |
const typename Data::MatrixXs & M = data.M; |
|
41 |
206 |
typename Data::MatrixXs & U = data.U; |
|
42 |
206 |
typename Data::VectorXs & D = data.D; |
|
43 |
206 |
typename Data::VectorXs & Dinv = data.Dinv; |
|
44 |
|||
45 |
✓✓ | 6784 |
for(int j=model.nv-1;j>=0;--j ) |
46 |
{ |
||
47 |
6578 |
const int NVT = data.nvSubtree_fromRow[(size_t)j]-1; |
|
48 |
✓✗ | 6578 |
typename Data::VectorXs::SegmentReturnType DUt = data.tmp.head(NVT); |
49 |
✓✓ | 6578 |
if(NVT) |
50 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
11508 |
DUt.noalias() = U.row(j).segment(j+1,NVT).transpose() |
51 |
✓✗ | 5754 |
.cwiseProduct(D.segment(j+1,NVT)); |
52 |
|||
53 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
6578 |
D[j] = M(j,j) - U.row(j).segment(j+1,NVT).dot(DUt); |
54 |
✓✗✓✗ |
6578 |
Dinv[j] = Scalar(1)/D[j]; |
55 |
✓✓ | 59157 |
for(int _i = data.parents_fromRow[(size_t)j]; _i >= 0;_i = data.parents_fromRow[(size_t)_i]) |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
52579 |
U(_i,j) = (M(_i,j) - U.row(_i).segment(j+1,NVT).dot(DUt)) * Dinv[j]; |
57 |
} |
||
58 |
|||
59 |
206 |
return data.U; |
|
60 |
} |
||
61 |
|||
62 |
namespace internal |
||
63 |
{ |
||
64 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
65 |
struct Uv |
||
66 |
{ |
||
67 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
68 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
69 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
70 |
const Eigen::MatrixBase<Mat> & m) |
||
71 |
{ |
||
72 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
||
73 |
for(int k = 0; k < m_.cols(); ++k) |
||
74 |
cholesky::Uv(model,data,m_.col(k)); |
||
75 |
} |
||
76 |
}; |
||
77 |
|||
78 |
template<typename Mat> |
||
79 |
struct Uv<Mat,1> |
||
80 |
{ |
||
81 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
82 |
4 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
83 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
84 |
const Eigen::MatrixBase<Mat> & v) |
||
85 |
{ |
||
86 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
87 |
|||
88 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
89 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
90 |
✓✗ | 4 |
assert(model.check(data) && "data is not consistent with model."); |
91 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
4 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
92 |
|||
93 |
4 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
94 |
|||
95 |
4 |
const typename Data::MatrixXs & U = data.U; |
|
96 |
4 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
97 |
|||
98 |
✓✓ | 128 |
for(int k=0;k < model.nv-1;++k) // You can stop one step before nv |
99 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
124 |
v_.row(k) += U.row(k).segment(k+1,nvt[(size_t)k]-1) * v_.middleRows(k+1,nvt[(size_t)k]-1); |
100 |
4 |
} |
|
101 |
}; |
||
102 |
|||
103 |
} // namespace internal |
||
104 |
|||
105 |
/* Compute U*v. |
||
106 |
* Nota: there is no smart way of doing U*D*v, so it is not proposed. */ |
||
107 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
108 |
4 |
Mat & Uv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
109 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
110 |
const Eigen::MatrixBase<Mat> & m) |
||
111 |
{ |
||
112 |
4 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
113 |
4 |
internal::Uv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_); |
|
114 |
4 |
return m_.derived(); |
|
115 |
} |
||
116 |
|||
117 |
namespace internal |
||
118 |
{ |
||
119 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
120 |
struct Utv |
||
121 |
{ |
||
122 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
123 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
124 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
125 |
const Eigen::MatrixBase<Mat> & m) |
||
126 |
{ |
||
127 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
||
128 |
for(int k = 0; k < m_.cols(); ++k) |
||
129 |
cholesky::Utv(model,data,m_.col(k)); |
||
130 |
} |
||
131 |
}; |
||
132 |
|||
133 |
template<typename Mat> |
||
134 |
struct Utv<Mat,1> |
||
135 |
{ |
||
136 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
137 |
3 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
138 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
139 |
const Eigen::MatrixBase<Mat> & v) |
||
140 |
{ |
||
141 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
142 |
|||
143 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
144 |
|||
145 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
146 |
✓✗ | 3 |
assert(model.check(data) && "data is not consistent with model."); |
147 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
148 |
3 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
149 |
|||
150 |
3 |
const typename Data::MatrixXs & U = data.U; |
|
151 |
3 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
152 |
|||
153 |
✓✓ | 96 |
for( int k=model.nv-2;k>=0;--k ) // You can start from nv-2 (no child in nv-1) |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
93 |
v_.segment(k+1,nvt[(size_t)k]-1) += U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*v_[k]; |
155 |
3 |
} |
|
156 |
}; |
||
157 |
|||
158 |
} // namespace internal |
||
159 |
|||
160 |
/* Compute U'*v */ |
||
161 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
162 |
3 |
Mat & Utv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
163 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
164 |
const Eigen::MatrixBase<Mat> & m) |
||
165 |
{ |
||
166 |
3 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
167 |
3 |
internal::Utv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_); |
|
168 |
3 |
return m_.derived(); |
|
169 |
} |
||
170 |
|||
171 |
namespace internal |
||
172 |
{ |
||
173 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
174 |
struct Uiv |
||
175 |
{ |
||
176 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
177 |
205 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
178 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
179 |
const Eigen::MatrixBase<Mat> & m) |
||
180 |
{ |
||
181 |
205 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
182 |
✓✓ | 1501 |
for(int k = 0; k < m_.cols(); ++k) |
183 |
✓✗ | 1296 |
cholesky::Uiv(model,data,m_.col(k)); |
184 |
205 |
} |
|
185 |
}; |
||
186 |
|||
187 |
template<typename Mat> |
||
188 |
struct Uiv<Mat,1> |
||
189 |
{ |
||
190 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
191 |
3650 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
192 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
193 |
const Eigen::MatrixBase<Mat> & v) |
||
194 |
{ |
||
195 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
196 |
|||
197 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
198 |
✓✗ | 3650 |
assert(model.check(data) && "data is not consistent with model."); |
199 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3650 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
200 |
3650 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
201 |
|||
202 |
3650 |
const typename Data::MatrixXs & U = data.U; |
|
203 |
3650 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
204 |
|||
205 |
✓✓ | 116436 |
for(int k=model.nv-2;k>=0;--k) // You can start from nv-2 (no child in nv-1) |
206 |
✓✗✓✗ ✓✗✓✗ |
112786 |
v_[k] -= U.row(k).segment(k+1,nvt[(size_t)k]-1).dot(v_.segment(k+1,nvt[(size_t)k]-1)); |
207 |
3650 |
} |
|
208 |
}; |
||
209 |
|||
210 |
} // namespace internal |
||
211 |
|||
212 |
/* Compute U^{-1}*v |
||
213 |
* Nota: there is no efficient way to compute D^{-1}U^{-1}v |
||
214 |
* in a single loop, so algorithm is not proposed.*/ |
||
215 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
216 |
4060 |
Mat & Uiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
217 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
218 |
const Eigen::MatrixBase<Mat> & m) |
||
219 |
{ |
||
220 |
4060 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
221 |
4060 |
internal::Uiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_); |
|
222 |
4060 |
return m_.derived(); |
|
223 |
} |
||
224 |
|||
225 |
namespace internal |
||
226 |
{ |
||
227 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
228 |
struct Utiv |
||
229 |
{ |
||
230 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
231 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
232 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
233 |
const Eigen::MatrixBase<Mat> & m) |
||
234 |
{ |
||
235 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
||
236 |
for(int k = 0; k < m_.cols(); ++k) |
||
237 |
cholesky::Utiv(model,data,m_.col(k)); |
||
238 |
} |
||
239 |
}; |
||
240 |
|||
241 |
template<typename Mat> |
||
242 |
struct Utiv<Mat,1> |
||
243 |
{ |
||
244 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
245 |
669 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
246 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
247 |
const Eigen::MatrixBase<Mat> & v) |
||
248 |
{ |
||
249 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
250 |
|||
251 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
252 |
✓✗ | 669 |
assert(model.check(data) && "data is not consistent with model."); |
253 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
669 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
254 |
669 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
255 |
|||
256 |
669 |
const typename Data::MatrixXs & U = data.U; |
|
257 |
669 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
258 |
|||
259 |
✓✓ | 21380 |
for(int k=0; k<model.nv-1; ++k) // You can stop one step before nv. |
260 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
20711 |
v_.segment(k+1,nvt[(size_t)k]-1) -= U.row(k).segment(k+1,nvt[(size_t)k]-1).transpose() * v_[k]; |
261 |
669 |
} |
|
262 |
}; |
||
263 |
|||
264 |
} // namespace internal |
||
265 |
|||
266 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
267 |
669 |
Mat & Utiv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
268 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
269 |
const Eigen::MatrixBase<Mat> & m) |
||
270 |
{ |
||
271 |
669 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
272 |
669 |
internal::Utiv<Mat,Mat::ColsAtCompileTime>::run(model,data,m_); |
|
273 |
669 |
return m_.derived(); |
|
274 |
} |
||
275 |
|||
276 |
namespace internal |
||
277 |
{ |
||
278 |
template<typename Mat, typename MatRes, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
279 |
struct Mv |
||
280 |
{ |
||
281 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
282 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
283 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
284 |
const Eigen::MatrixBase<Mat> & min, |
||
285 |
const Eigen::MatrixBase<MatRes> & mout |
||
286 |
) |
||
287 |
{ |
||
288 |
MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout); |
||
289 |
for(int k = 0; k < min.cols(); ++k) |
||
290 |
cholesky::Mv(model,data,min.col(k),mout_.col(k)); |
||
291 |
} |
||
292 |
}; |
||
293 |
|||
294 |
template<typename Mat, typename MatRes> |
||
295 |
struct Mv<Mat,MatRes,1> |
||
296 |
{ |
||
297 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
298 |
2 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
299 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
300 |
const Eigen::MatrixBase<Mat> & vin, |
||
301 |
const Eigen::MatrixBase<MatRes> & vout) |
||
302 |
{ |
||
303 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
304 |
|||
305 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
306 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRes) |
||
307 |
|||
308 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
309 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
310 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv); |
311 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv); |
312 |
|||
313 |
2 |
MatRes & vout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,vout); |
|
314 |
|||
315 |
2 |
const typename Data::MatrixXs & M = data.M; |
|
316 |
2 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
317 |
|||
318 |
✓✓ | 66 |
for(int k=model.nv-1;k>=0;--k) |
319 |
{ |
||
320 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
vout_[k] = M.row(k).segment(k,nvt[(size_t)k]) * vin.segment(k,nvt[(size_t)k]); |
321 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
vout_.segment(k+1,nvt[(size_t)k]-1) += M.row(k).segment(k+1,nvt[(size_t)k]-1).transpose()*vin[k]; |
322 |
} |
||
323 |
2 |
} |
|
324 |
}; |
||
325 |
|||
326 |
} // namespace internal |
||
327 |
|||
328 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat, typename MatRes> |
||
329 |
2 |
MatRes & Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
330 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
331 |
const Eigen::MatrixBase<Mat> & min, |
||
332 |
const Eigen::MatrixBase<MatRes> & mout) |
||
333 |
{ |
||
334 |
2 |
MatRes & mout_ = PINOCCHIO_EIGEN_CONST_CAST(MatRes,mout); |
|
335 |
2 |
internal::Mv<Mat,MatRes,Mat::ColsAtCompileTime>::run(model,data,min.derived(),mout_); |
|
336 |
2 |
return mout_.derived(); |
|
337 |
} |
||
338 |
|||
339 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
340 |
2 |
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) Mv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
341 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
342 |
const Eigen::MatrixBase<Mat> & min) |
||
343 |
{ |
||
344 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Mat) ReturnType; |
||
345 |
✓✗✓✗ |
4 |
ReturnType res(model.nv,min.cols()); |
346 |
✓✗✓✗ |
4 |
return Mv(model,data,min,res); |
347 |
} |
||
348 |
|||
349 |
namespace internal |
||
350 |
{ |
||
351 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
352 |
struct UDUtv |
||
353 |
{ |
||
354 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
355 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
||
356 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
357 |
const Eigen::MatrixBase<Mat> & m) |
||
358 |
{ |
||
359 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
||
360 |
for(int k = 0; k < m_.cols(); ++k) |
||
361 |
cholesky::UDUtv(model,data,m_.col(k)); |
||
362 |
} |
||
363 |
}; |
||
364 |
|||
365 |
template<typename Mat> |
||
366 |
struct UDUtv<Mat,1> |
||
367 |
{ |
||
368 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
369 |
2 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
370 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
371 |
const Eigen::MatrixBase<Mat> & v) |
||
372 |
{ |
||
373 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
374 |
|||
375 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
376 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
377 |
|||
378 |
2 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
379 |
|||
380 |
2 |
cholesky::Utv(model,data,v_); |
|
381 |
✓✗✓✗ |
2 |
v_.array() *= data.D.array(); |
382 |
2 |
cholesky::Uv(model,data,v_); |
|
383 |
2 |
} |
|
384 |
}; |
||
385 |
|||
386 |
} // namespace internal |
||
387 |
|||
388 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
389 |
2 |
Mat & UDUtv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
390 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
391 |
const Eigen::MatrixBase<Mat> & m) |
||
392 |
{ |
||
393 |
2 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
394 |
|||
395 |
2 |
internal::UDUtv<Mat>::run(model,data,m_); |
|
396 |
2 |
return m_; |
|
397 |
} |
||
398 |
|||
399 |
namespace internal |
||
400 |
{ |
||
401 |
template<typename Mat, int ColsAtCompileTime = Mat::ColsAtCompileTime> |
||
402 |
struct solve |
||
403 |
{ |
||
404 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
405 |
3 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
406 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
407 |
const Eigen::MatrixBase<Mat> & m) |
||
408 |
{ |
||
409 |
3 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
410 |
✓✓ | 99 |
for(int k = 0; k < m_.cols(); ++k) |
411 |
✓✗ | 96 |
cholesky::solve(model,data,m_.col(k)); |
412 |
3 |
} |
|
413 |
}; |
||
414 |
|||
415 |
template<typename Mat> |
||
416 |
struct solve<Mat,1> |
||
417 |
{ |
||
418 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
419 |
636 |
static void run(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
420 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
421 |
const Eigen::MatrixBase<Mat> & v) |
||
422 |
{ |
||
423 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) |
||
424 |
|||
425 |
✓✗ | 636 |
assert(model.check(data) && "data is not consistent with model."); |
426 |
|||
427 |
636 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
428 |
|||
429 |
636 |
cholesky::Uiv(model,data,v_); |
|
430 |
✓✗✓✗ |
636 |
v_.array() *= data.Dinv.array(); |
431 |
636 |
cholesky::Utiv(model,data,v_); |
|
432 |
636 |
} |
|
433 |
}; |
||
434 |
|||
435 |
} // namespace internal |
||
436 |
|||
437 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
438 |
533 |
Mat & solve(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
439 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
440 |
const Eigen::MatrixBase<Mat> & m) |
||
441 |
{ |
||
442 |
533 |
Mat & m_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,m); |
|
443 |
533 |
internal::solve<Mat,Mat::ColsAtCompileTime>::run(model,data,m_); |
|
444 |
533 |
return m_.derived(); |
|
445 |
} |
||
446 |
|||
447 |
namespace internal |
||
448 |
{ |
||
449 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
450 |
256 |
Mat & Miunit(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
451 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
452 |
const int col, |
||
453 |
const Eigen::MatrixBase<Mat> & v) |
||
454 |
{ |
||
455 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
456 |
|||
457 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
458 |
✓✗ | 256 |
assert(model.check(data) && "data is not consistent with model."); |
459 |
✓✗✗✓ ✗✗ |
256 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0); |
460 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
256 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
461 |
|||
462 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
463 |
|||
464 |
256 |
const typename Data::MatrixXs & U = data.U; |
|
465 |
256 |
const std::vector<int> & nvt = data.nvSubtree_fromRow; |
|
466 |
256 |
Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,v); |
|
467 |
|||
468 |
256 |
const int last_col = std::min<int>(col-1,model.nv-2); // You can start from nv-2 (no child in nv-1) |
|
469 |
✓✗ | 256 |
v_.tail(model.nv - col - 1).setZero(); |
470 |
256 |
v_[col] = 1.; |
|
471 |
✓✓ | 4224 |
for( int k=last_col;k>=0;--k ) |
472 |
{ |
||
473 |
3968 |
int nvt_max = std::min<int>(col,nvt[(size_t)k]-1); |
|
474 |
✓✗✓✗ ✓✗✓✗ |
3968 |
v_[k] = -U.row(k).segment(k+1,nvt_max).dot(v_.segment(k+1,nvt_max)); |
475 |
} |
||
476 |
|||
477 |
✓✗✓✗ ✓✗✓✗ |
256 |
v_.head(col+1).array() *= data.Dinv.head(col+1).array(); |
478 |
|||
479 |
✓✓ | 8192 |
for( int k=0;k<model.nv-1;++k ) // You can stop one step before nv. |
480 |
{ |
||
481 |
7936 |
int nvt_max = nvt[(size_t)k]-1; |
|
482 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
7936 |
v_.segment(k+1,nvt_max) -= U.row(k).segment(k+1,nvt_max).transpose() * v_[k]; |
483 |
} |
||
484 |
|||
485 |
256 |
return v_.derived(); |
|
486 |
} |
||
487 |
}// namespace internal |
||
488 |
|||
489 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Mat> |
||
490 |
3 |
Mat & computeMinv(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
491 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
492 |
const Eigen::MatrixBase<Mat> & Minv) |
||
493 |
{ |
||
494 |
✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.rows(), model.nv); |
495 |
✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
3 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv); |
496 |
|||
497 |
✗✗ | 3 |
assert(model.check(data) && "data is not consistent with model."); |
498 |
|||
499 |
3 |
Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat,Minv); |
|
500 |
|||
501 |
✗✗ | 99 |
for(int k = 0; k < model.nv; ++k) |
502 |
✗✗ | 96 |
internal::Miunit(model,data,k,Minv_.col(k)); |
503 |
|||
504 |
3 |
return Minv_.derived(); |
|
505 |
} |
||
506 |
|||
507 |
} // namespace cholesky |
||
508 |
} // namespace pinocchio |
||
509 |
|||
510 |
/// @endcond |
||
511 |
|||
512 |
#endif // ifndef __pinocchio_cholesky_hxx__ |
Generated by: GCOVR (Version 4.2) |