GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_spatial_log_hxx__ |
||
6 |
#define __pinocchio_spatial_log_hxx__ |
||
7 |
|||
8 |
namespace pinocchio |
||
9 |
{ |
||
10 |
/// \brief Generic evaluation of log3 function |
||
11 |
template<typename _Scalar> |
||
12 |
struct log3_impl |
||
13 |
{ |
||
14 |
template<typename Matrix3Like, typename Vector3Out> |
||
15 |
21810 |
static void run(const Eigen::MatrixBase<Matrix3Like> & R, |
|
16 |
typename Matrix3Like::Scalar & theta, |
||
17 |
const Eigen::MatrixBase<Vector3Out> & res) |
||
18 |
{ |
||
19 |
✓✗✓✗ |
21810 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3); |
20 |
✓✗✓✗ |
21810 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, res, 3, 1); |
21 |
// TODO: add static_assert<is_same_type<_Scalar,Scalar> |
||
22 |
|||
23 |
typedef typename Matrix3Like::Scalar Scalar; |
||
24 |
typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3; |
||
25 |
|||
26 |
✓✓✓✗ |
21810 |
static const Scalar PI_value = PI<Scalar>(); |
27 |
|||
28 |
21810 |
Scalar tr = R.trace(); |
|
29 |
✓✓ | 21810 |
if(tr >= Scalar(3)) |
30 |
{ |
||
31 |
12557 |
tr = Scalar(3); // clip value |
|
32 |
12557 |
theta = Scalar(0); // acos((3-1)/2) |
|
33 |
} |
||
34 |
✗✓ | 9253 |
else if(tr <= Scalar(-1)) |
35 |
{ |
||
36 |
tr = Scalar(-1); // clip value |
||
37 |
theta = PI_value; // acos((-1-1)/2) |
||
38 |
} |
||
39 |
else |
||
40 |
9253 |
theta = math::acos((tr - Scalar(1))/Scalar(2)); |
|
41 |
✗✓ | 21810 |
assert(theta == theta && "theta contains some NaN"); // theta != NaN |
42 |
|||
43 |
21810 |
Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res); |
|
44 |
|||
45 |
// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small. |
||
46 |
✓✓ | 21810 |
if(theta >= PI_value - 1e-2) |
47 |
{ |
||
48 |
// 1e-2: A low value is not required since the computation is |
||
49 |
// using explicit formula. However, the precision of this method |
||
50 |
// is the square root of the precision with the antisymmetric |
||
51 |
// method (Nominal case). |
||
52 |
3 |
const Scalar cphi = -(tr - Scalar(1))/Scalar(2); |
|
53 |
3 |
const Scalar beta = theta*theta / (Scalar(1) + cphi); |
|
54 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
const Vector3 tmp((R.diagonal().array() + cphi) * beta); |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
res_(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0)); |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
res_(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0)); |
57 |
✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗ |
3 |
res_(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0)); |
58 |
} |
||
59 |
else |
||
60 |
{ |
||
61 |
21807 |
const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>()) |
|
62 |
✓✓ | 21807 |
? theta / sin(theta) |
63 |
: Scalar(1)) / Scalar(2); |
||
64 |
21807 |
res_(0) = t * (R (2, 1) - R (1, 2)); |
|
65 |
21807 |
res_(1) = t * (R (0, 2) - R (2, 0)); |
|
66 |
21807 |
res_(2) = t * (R (1, 0) - R (0, 1)); |
|
67 |
} |
||
68 |
21810 |
} |
|
69 |
}; |
||
70 |
|||
71 |
/// \brief Generic evaluation of Jlog3 function |
||
72 |
template<typename _Scalar> |
||
73 |
struct Jlog3_impl |
||
74 |
{ |
||
75 |
template<typename Scalar, typename Vector3Like, typename Matrix3Like> |
||
76 |
992 |
static void run(const Scalar & theta, |
|
77 |
const Eigen::MatrixBase<Vector3Like> & log, |
||
78 |
const Eigen::MatrixBase<Matrix3Like> & Jlog) |
||
79 |
{ |
||
80 |
✓✗✓✗ ✓✗✓✗ |
992 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, log, 3, 1); |
81 |
✓✗✓✓ ✗✓✗✓ ✗ |
992 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3); |
82 |
// TODO: add static_assert<is_same_type<_Scalar,Scalar> |
||
83 |
|||
84 |
992 |
Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog); |
|
85 |
|||
86 |
Scalar alpha, diag_value; |
||
87 |
✓✗✓✓ |
992 |
if(theta < TaylorSeriesExpansion<Scalar>::template precision<3>()) |
88 |
{ |
||
89 |
110 |
alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720); |
|
90 |
110 |
diag_value = Scalar(0.5) * (2 - theta*theta / Scalar(6)); |
|
91 |
} |
||
92 |
else |
||
93 |
{ |
||
94 |
// Jlog = alpha I |
||
95 |
882 |
Scalar ct,st; SINCOS(theta,&st,&ct); |
|
96 |
882 |
const Scalar st_1mct = st/(Scalar(1)-ct); |
|
97 |
|||
98 |
882 |
alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta); |
|
99 |
882 |
diag_value = Scalar(0.5) * (theta * st_1mct); |
|
100 |
} |
||
101 |
|||
102 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
992 |
Jlog_.noalias() = alpha * log * log.transpose(); |
103 |
✓✗✓✗ ✓✗ |
992 |
Jlog_.diagonal().array() += diag_value; |
104 |
|||
105 |
// Jlog += r_{\times}/2 |
||
106 |
✓✗✓✗ |
992 |
addSkew(Scalar(0.5) * log, Jlog_); |
107 |
992 |
} |
|
108 |
}; |
||
109 |
|||
110 |
/// \brief Generic evaluation of log6 function |
||
111 |
template<typename _Scalar> |
||
112 |
struct log6_impl |
||
113 |
{ |
||
114 |
template<typename Scalar, int Options, typename MotionDerived> |
||
115 |
19397 |
static void run(const SE3Tpl<Scalar,Options> & M, |
|
116 |
MotionDense<MotionDerived> & mout) |
||
117 |
{ |
||
118 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
119 |
typedef typename SE3::Vector3 Vector3; |
||
120 |
|||
121 |
✓✗ | 19397 |
typename SE3::ConstAngularRef R = M.rotation(); |
122 |
✓✗ | 19397 |
typename SE3::ConstLinearRef p = M.translation(); |
123 |
|||
124 |
Scalar t; |
||
125 |
✓✗ | 19397 |
Vector3 w(log3(R,t)); // t in [0,π] |
126 |
19397 |
const Scalar t2 = t*t; |
|
127 |
Scalar alpha, beta; |
||
128 |
|||
129 |
✓✗✓✓ |
19397 |
if (t < TaylorSeriesExpansion<Scalar>::template precision<3>()) |
130 |
{ |
||
131 |
15811 |
alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720); |
|
132 |
15811 |
beta = Scalar(1)/Scalar(12) + t2/Scalar(720); |
|
133 |
} |
||
134 |
else |
||
135 |
{ |
||
136 |
3586 |
Scalar st,ct; SINCOS(t,&st,&ct); |
|
137 |
3586 |
alpha = t*st/(Scalar(2)*(Scalar(1)-ct)); |
|
138 |
3586 |
beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct)); |
|
139 |
} |
||
140 |
|||
141 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
19397 |
mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w; |
142 |
✓✗✓✗ |
19397 |
mout.angular() = w; |
143 |
19397 |
} |
|
144 |
}; |
||
145 |
|||
146 |
template<typename _Scalar> |
||
147 |
struct Jlog6_impl |
||
148 |
{ |
||
149 |
template<typename Scalar, int Options, typename Matrix6Like> |
||
150 |
404 |
static void run(const SE3Tpl<Scalar, Options> & M, |
|
151 |
const Eigen::MatrixBase<Matrix6Like> & Jlog) |
||
152 |
{ |
||
153 |
✓✗✓✗ ✓✗✓✗ |
404 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6); |
154 |
// TODO: add static_assert<is_same_type<_Scalar,Scalar> |
||
155 |
|||
156 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
157 |
typedef typename SE3::Vector3 Vector3; |
||
158 |
404 |
Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog); |
|
159 |
|||
160 |
✓✗ | 404 |
typename SE3::ConstAngularRef R = M.rotation(); |
161 |
✓✗ | 404 |
typename SE3::ConstLinearRef p = M.translation(); |
162 |
|||
163 |
Scalar t; |
||
164 |
✓✗ | 404 |
Vector3 w(log3(R,t)); |
165 |
|||
166 |
// value is decomposed as following: |
||
167 |
// value = [ A, B; |
||
168 |
// C, D ] |
||
169 |
typedef Eigen::Block<Matrix6Like,3,3> Block33; |
||
170 |
✓✗ | 404 |
Block33 A = value.template topLeftCorner<3,3>(); |
171 |
✓✗ | 404 |
Block33 B = value.template topRightCorner<3,3>(); |
172 |
✓✗ | 404 |
Block33 C = value.template bottomLeftCorner<3,3>(); |
173 |
✓✗ | 404 |
Block33 D = value.template bottomRightCorner<3,3>(); |
174 |
|||
175 |
✓✗ | 404 |
Jlog3(t, w, A); |
176 |
✓✗ | 404 |
D = A; |
177 |
|||
178 |
404 |
const Scalar t2 = t*t; |
|
179 |
Scalar beta, beta_dot_over_theta; |
||
180 |
✓✗✓✓ |
404 |
if(t < TaylorSeriesExpansion<Scalar>::template precision<3>()) |
181 |
{ |
||
182 |
106 |
beta = Scalar(1)/Scalar(12) + t2/Scalar(720); |
|
183 |
106 |
beta_dot_over_theta = Scalar(1)/Scalar(360); |
|
184 |
} |
||
185 |
else |
||
186 |
{ |
||
187 |
298 |
const Scalar tinv = Scalar(1)/t, |
|
188 |
298 |
t2inv = tinv*tinv; |
|
189 |
298 |
Scalar st,ct; SINCOS (t, &st, &ct); |
|
190 |
298 |
const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct)); |
|
191 |
|||
192 |
298 |
beta = t2inv - st*tinv*inv_2_2ct; |
|
193 |
298 |
beta_dot_over_theta = -Scalar(2)*t2inv*t2inv + |
|
194 |
298 |
(Scalar(1) + st*tinv) * t2inv * inv_2_2ct; |
|
195 |
} |
||
196 |
|||
197 |
✓✗ | 404 |
Scalar wTp = w.dot(p); |
198 |
|||
199 |
✓✗✓✗ ✓✗✓✗ |
404 |
Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p); |
200 |
// C can be treated as a temporary variable |
||
201 |
✓✗✓✗ ✓✗✓✗ |
404 |
C.noalias() = v3_tmp * w.transpose(); |
202 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
404 |
C.noalias() += beta * w * p.transpose(); |
203 |
✓✗✓✗ ✓✗ |
404 |
C.diagonal().array() += wTp * beta; |
204 |
✓✗✓✗ |
404 |
addSkew(Scalar(.5)*p,C); |
205 |
|||
206 |
✓✗✓✗ ✓✗ |
404 |
B.noalias() = C * A; |
207 |
✓✗ | 404 |
C.setZero(); |
208 |
404 |
} |
|
209 |
}; |
||
210 |
|||
211 |
} // namespace pinocchio |
||
212 |
|||
213 |
#endif // ifndef __pinocchio_spatial_log_hxx__ |
Generated by: GCOVR (Version 4.2) |