GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/log.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 150 165 90.9%
Branches: 206 406 50.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_log_hxx__
6 #define __pinocchio_spatial_log_hxx__
7
8 namespace pinocchio
9 {
10
11 namespace internal
12 {
13 template<long i0, typename Matrix3, typename Vector3>
14 8358 void compute_theta_axis(
15 const typename Matrix3::Scalar & val,
16 const Eigen::MatrixBase<Matrix3> & R,
17 typename Matrix3::Scalar & angle,
18 const Eigen::MatrixBase<Vector3> & _axis)
19 {
20 typedef typename Matrix3::Scalar Scalar;
21 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
22
23 static const long i1 = (i0 + 1) % 3;
24 static const long i2 = (i0 + 2) % 3;
25 8358 Vector3 & axis = _axis.const_cast_derived();
26
27 8358 const Scalar s =
28 8358 math::sqrt(val + eps + eps * eps)
29 16716 * if_then_else(
30
2/4
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
 8358 GE, R.coeff(i2, i1), R.coeff(i1, i2), Scalar(1.),
31
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 8358 Scalar(-1.)); // Ensure value in sqrt is non negative and that s is non zero
32
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 8358 axis[i0] = s / Scalar(2);
33
3/6
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4179 times.
✗ Branch 8 not taken.
 8358 axis[i1] = Scalar(1) / (2 * s) * (R.coeff(i1, i0) + R.coeff(i0, i1));
34
3/6
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4179 times.
✗ Branch 8 not taken.
 8358 axis[i2] = Scalar(1) / (2 * s) * (R.coeff(i2, i0) + R.coeff(i0, i2));
35
2/4
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
 8358 const Scalar w = Scalar(1) / (2 * s) * (R.coeff(i2, i1) - R.coeff(i1, i2));
36
37
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 8358 const Scalar axis_norm = axis.norm();
38
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 8358 angle = 2 * math::atan2(axis_norm, w);
39
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 8358 axis /= axis_norm;
40 8358 }
41 } // namespace internal
42
43 /// \brief Renormalize a rotation matrix.
44 template<typename Matrix3>
45 inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
46 1393 renormalize_rotation_matrix(const Eigen::MatrixBase<Matrix3> & R)
47 {
48 1393 typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) Rout;
49
6/12
✓ Branch 2 taken 1393 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1393 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1393 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1393 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1393 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1393 times.
✗ Branch 18 not taken.
 1393 Rout.col(0).noalias() = R.col(0) / R.col(0).norm();
50
6/12
✓ Branch 2 taken 1393 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1393 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1393 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1393 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1393 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1393 times.
✗ Branch 18 not taken.
 1393 Rout.col(1).noalias() = R.col(1) / R.col(1).norm();
51
5/10
✓ Branch 2 taken 1393 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1393 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1393 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1393 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1393 times.
✗ Branch 15 not taken.
 1393 Rout.col(2).noalias() = Rout.col(0).cross(Rout.col(1));
52
5/10
✓ Branch 2 taken 1393 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1393 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1393 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1393 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1393 times.
✗ Branch 15 not taken.
 1393 Rout.col(0).noalias() = Rout.col(1).cross(Rout.col(2));
53 1393 return Rout;
54 }
55
56 /// \brief Generic evaluation of log3 function
57 template<typename _Scalar>
58 struct log3_impl
59 {
60 template<typename Matrix3Like, typename Vector3Out>
61 1393 static void run(
62 const Eigen::MatrixBase<Matrix3Like> & R,
63 typename Matrix3Like::Scalar & theta,
64 const Eigen::MatrixBase<Vector3Out> & angle_axis)
65 {
66
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
67
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, angle_axis, 3, 1);
68 using namespace internal;
69
70 typedef typename Matrix3Like::Scalar Scalar;
71 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
72 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
73
74
3/4
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1388 times.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 1393 const static Scalar PI_value = PI<Scalar>();
75 1393 Vector3Out & angle_axis_ = angle_axis.const_cast_derived();
76
77 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Matrix3;
78
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 const Matrix3 Rnormed = renormalize_rotation_matrix(R);
79
80
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 const Scalar tr = Rnormed.trace();
81 1393 const Scalar cos_value = (tr - Scalar(1)) / Scalar(2);
82
83
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 const Scalar prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
84 // Singular cases when theta == PI
85
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 Vector3 angle_axis_singular;
86 Scalar theta_singular;
87
88 {
89
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 Vector3 val_singular;
90
7/14
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1393 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1393 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1393 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1393 times.
✗ Branch 20 not taken.
 1393 val_singular.array() = 2 * Rnormed.diagonal().array() - tr + Scalar(1);
91
3/6
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
 1393 Vector3 axis_0, axis_1, axis_2;
92 Scalar theta_0, theta_1, theta_2;
93
94
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 internal::compute_theta_axis<0>(val_singular[0], Rnormed, theta_0, axis_0);
95
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 internal::compute_theta_axis<1>(val_singular[1], Rnormed, theta_1, axis_1);
96
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 internal::compute_theta_axis<2>(val_singular[2], Rnormed, theta_2, axis_2);
97
98 2786 theta_singular = if_then_else(
99
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
 1393 GE, val_singular[0], val_singular[1],
100
3/6
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
 1393 if_then_else(GE, val_singular[0], val_singular[2], theta_0, theta_2),
101
4/8
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1393 times.
✗ Branch 11 not taken.
 1393 if_then_else(GE, val_singular[1], val_singular[2], theta_1, theta_2));
102
103
2/2
✓ Branch 0 taken 4179 times.
✓ Branch 1 taken 1393 times.
 5572 for (int k = 0; k < 3; ++k)
104
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 4179 angle_axis_singular[k] = if_then_else(
105
2/4
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
 4179 GE, val_singular[0], val_singular[1],
106
5/10
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4179 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4179 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4179 times.
✗ Branch 14 not taken.
 4179 if_then_else(GE, val_singular[0], val_singular[2], axis_0[k], axis_2[k]),
107
6/12
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4179 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4179 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4179 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4179 times.
✗ Branch 17 not taken.
 8358 if_then_else(GE, val_singular[1], val_singular[2], axis_1[k], axis_2[k]));
108 }
109 1393 const Scalar acos_expansion = math::sqrt(2 * (1 - cos_value) + eps * eps);
110 2786 const Scalar theta_nominal = if_then_else(
111 ✗ LE, tr, static_cast<Scalar>(Scalar(3) - prec),
112
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 if_then_else(
113 ✗ GE, tr, static_cast<Scalar>(Scalar(-1) + prec),
114
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 math::acos(cos_value), // then
115 ✗ static_cast<Scalar>(PI_value - acos_expansion) // else
116 ),
117 1393 static_cast<Scalar>(acos_expansion) // else
118 );
119
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1393 times.
✗ Branch 4 not taken.
 1393 assert(
120 check_expression_if_real<Scalar>(theta_nominal == theta_nominal)
121 && "theta contains some NaN"); // theta != NaN
122
123
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 Vector3 antisymmetric_R;
124
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 unSkew(Rnormed, antisymmetric_R);
125
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm();
126
127 2786 const Scalar t = if_then_else(
128 GE, theta_nominal, prec,
129 ✗ static_cast<Scalar>(theta_nominal / sin(theta_nominal)), // then
130 ✗ static_cast<Scalar>(
131 1393 Scalar(1.) + norm_antisymmetric_R_squared / Scalar(6)
132 1393 + norm_antisymmetric_R_squared * norm_antisymmetric_R_squared * Scalar(3)
133
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 / Scalar(40)) // else
134 );
135
136 2786 theta = if_then_else(
137
1/2
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
 1393 GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec), theta_nominal, theta_singular);
138
139
2/2
✓ Branch 0 taken 4179 times.
✓ Branch 1 taken 1393 times.
 5572 for (int k = 0; k < 3; ++k)
140
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 4179 angle_axis_[k] = if_then_else(
141 ✗ GE, cos_value, static_cast<Scalar>(Scalar(-1.) + prec),
142
1/2
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
 4179 static_cast<Scalar>(t * antisymmetric_R[k]),
143
2/4
✓ Branch 1 taken 4179 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4179 times.
✗ Branch 5 not taken.
 8358 static_cast<Scalar>(theta_singular * angle_axis_singular[k]));
144 1393 }
145 };
146
147 /// \brief Generic evaluation of Jlog3 function
148 template<typename _Scalar>
149 struct Jlog3_impl
150 {
151 template<typename Scalar, typename Vector3Like, typename Matrix3Like>
152 227 static void run(
153 const Scalar & theta,
154 const Eigen::MatrixBase<Vector3Like> & log,
155 const Eigen::MatrixBase<Matrix3Like> & Jlog)
156 {
157
2/4
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
 227 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, log, 3, 1);
158
2/4
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
 227 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3);
159
160 using namespace internal;
161 Scalar ct, st;
162 227 SINCOS(theta, &st, &ct);
163 227 const Scalar st_1mct = st / (Scalar(1) - ct);
164
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 227 const Scalar prec = TaylorSeriesExpansion<Scalar>::template precision<3>();
165
166 454 const Scalar alpha = if_then_else(
167 LT, theta, prec,
168 ✗ static_cast<Scalar>(Scalar(1) / Scalar(12) + theta * theta / Scalar(720)), // then
169
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 227 static_cast<Scalar>(Scalar(1) / (theta * theta) - st_1mct / (Scalar(2) * theta)) // else
170 );
171
172 454 const Scalar diag_value = if_then_else(
173 LT, theta, prec,
174 ✗ static_cast<Scalar>(Scalar(0.5) * (2 - theta * theta / Scalar(6))), // then
175
1/2
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
 227 static_cast<Scalar>(Scalar(0.5) * (theta * st_1mct)) // else
176 );
177
178 227 Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog);
179
5/10
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 128 times.
✗ Branch 14 not taken.
 227 Jlog_.noalias() = alpha * log * log.transpose();
180
3/6
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
 227 Jlog_.diagonal().array() += diag_value;
181
182 // Jlog += r_{\times}/2
183
2/4
✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
 227 addSkew(Scalar(0.5) * log, Jlog_);
184 227 }
185 };
186
187 /// \brief Generic evaluation of log6 function
188 template<typename _Scalar>
189 struct log6_impl
190 {
191 template<typename Scalar, int Options, typename MotionDerived>
192 1266 static void run(const SE3Tpl<Scalar, Options> & M, MotionDense<MotionDerived> & mout)
193 {
194 typedef SE3Tpl<Scalar, Options> SE3;
195 typedef typename SE3::Vector3 Vector3;
196
197
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 typename SE3::ConstAngularRef R = M.rotation();
198
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 typename SE3::ConstLinearRef p = M.translation();
199
200 using namespace internal;
201
202
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 Vector3 antisymmetric_R;
203
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 unSkew(R, antisymmetric_R);
204
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 const Scalar norm_antisymmetric_R_squared = antisymmetric_R.squaredNorm();
205
206 Scalar theta;
207
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 const Scalar tr = R.trace();
208
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 const Vector3 w(log3(R, theta)); // t in [0,Ï€]
209 1266 const Scalar & t2 = norm_antisymmetric_R_squared;
210
211 Scalar st, ct;
212 1266 SINCOS(theta, &st, &ct);
213 2532 const Scalar alpha = if_then_else(
214 GE, tr,
215
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 static_cast<Scalar>(Scalar(3) - TaylorSeriesExpansion<Scalar>::template precision<2>()),
216 ✗ static_cast<Scalar>(Scalar(1) - t2 / Scalar(12) - t2 * t2 / Scalar(720)), // then
217
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 static_cast<Scalar>(theta * st / (Scalar(2) * (Scalar(1) - ct))) // else
218 );
219
220 2532 const Scalar beta = if_then_else(
221 GE, tr,
222
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 static_cast<Scalar>(Scalar(3) - TaylorSeriesExpansion<Scalar>::template precision<2>()),
223 ✗ static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), // then
224 ✗ static_cast<Scalar>(
225
1/2
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
 1266 Scalar(1) / (theta * theta) - st / (Scalar(2) * theta * (Scalar(1) - ct))) // else
226 );
227
228
10/20
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1266 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1266 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1266 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1266 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1266 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1266 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1266 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1266 times.
✗ Branch 29 not taken.
 1266 mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w;
229
2/4
✓ Branch 1 taken 1266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1266 times.
✗ Branch 5 not taken.
 1266 mout.angular() = w;
230 1266 }
231
232 template<typename Vector3Like, typename QuaternionLike, typename MotionDerived>
233 130 static void run(
234 const Eigen::QuaternionBase<QuaternionLike> & quat,
235 const Eigen::MatrixBase<Vector3Like> & vec,
236 MotionDense<MotionDerived> & mout)
237 {
238
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, vec, 3, 1);
239
240 typedef typename Vector3Like::Scalar Scalar;
241 enum
242 {
243 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options
244 };
245 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
246 130 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
247
248 using namespace internal;
249
250
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));
251
252 Scalar theta;
253
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 Vector3 w(quaternion::log3(quat, theta)); // theta nonsingular by construction
254
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 const Scalar t2 = w.squaredNorm();
255
256 // Scalar st,ct; SINCOS(theta,&st,&ct);
257 Scalar st_2, ct_2;
258
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 ct_2 = pos_neg * quat.w();
259
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 st_2 = math::sqrt(quat.vec().squaredNorm() + eps * eps);
260 130 const Scalar cot_th_2 = ct_2 / st_2;
261 // const Scalar cot_th_2 = ( st / (Scalar(1) - ct) ); // cotan of half angle
262
263 // we use formula (9.26) from
264 // https://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf for the linear
265 // part of the Log map. A Taylor series expansion of cotan can be used up to order 4
266 130 const Scalar th_2_squared = t2 / Scalar(4); // (theta / 2) squared
267
268 // const Scalar alpha = if_then_else(LE,theta,TaylorSeriesExpansion<Scalar>::template
269 // precision<3>(),
270 // static_cast<Scalar>(Scalar(1) - t2/Scalar(12) -
271 // t2*t2/Scalar(720)), // then
272 // static_cast<Scalar>(theta * cot_th_2 /(Scalar(2)))
273 // // else
274 // );
275
276 130 const Scalar beta_alt = (Scalar(1) / Scalar(3) - th_2_squared / Scalar(45)) / Scalar(4);
277 260 const Scalar beta = if_then_else(
278
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 LE, theta, TaylorSeriesExpansion<Scalar>::template precision<3>(),
279 ✗ static_cast<Scalar>(beta_alt), // then
280
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 130 static_cast<Scalar>(Scalar(1) / t2 - cot_th_2 * Scalar(0.5) / theta) // else
281 // static_cast<Scalar>(Scalar(1) / t2 - st/(Scalar(2)*theta*(Scalar(1)-ct))) // else
282 );
283
284 // mout.linear().noalias() = alpha * vec - Scalar(0.5) * w.cross(vec) + (beta * w.dot(vec)) *
285 // w;
286
10/20
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 130 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 130 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 130 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 130 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 130 times.
✗ Branch 29 not taken.
 130 mout.linear().noalias() = vec - Scalar(0.5) * w.cross(vec) + beta * w.cross(w.cross(vec));
287
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
 130 mout.angular() = w;
288 130 }
289 };
290
291 template<typename _Scalar>
292 struct Jlog6_impl
293 {
294 template<typename Scalar, int Options, typename Matrix6Like>
295 124 static void run(const SE3Tpl<Scalar, Options> & M, const Eigen::MatrixBase<Matrix6Like> & Jlog)
296 {
297
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 124 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6);
298
299 typedef SE3Tpl<Scalar, Options> SE3;
300 typedef typename SE3::Vector3 Vector3;
301 124 Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, Jlog);
302
303
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 typename SE3::ConstAngularRef R = M.rotation();
304
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 typename SE3::ConstLinearRef p = M.translation();
305
306 using namespace internal;
307
308 Scalar t;
309
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Vector3 w(log3(R, t));
310
311 // value is decomposed as following:
312 // value = [ A, B;
313 // C, D ]
314 typedef Eigen::Block<Matrix6Like, 3, 3> Block33;
315
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Block33 A = value.template topLeftCorner<3, 3>();
316
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Block33 B = value.template topRightCorner<3, 3>();
317
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Block33 C = value.template bottomLeftCorner<3, 3>();
318
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Block33 D = value.template bottomRightCorner<3, 3>();
319
320
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 Jlog3(t, w, A);
321
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 D = A;
322
323 124 const Scalar t2 = t * t;
324 124 const Scalar tinv = Scalar(1) / t, t2inv = tinv * tinv;
325
326 Scalar st, ct;
327 124 SINCOS(t, &st, &ct);
328 124 const Scalar inv_2_2ct = Scalar(1) / (Scalar(2) * (Scalar(1) - ct));
329
330 248 const Scalar beta = if_then_else(
331
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
332 ✗ static_cast<Scalar>(Scalar(1) / Scalar(12) + t2 / Scalar(720)), // then
333
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 static_cast<Scalar>(t2inv - st * tinv * inv_2_2ct) // else
334 );
335
336 248 const Scalar beta_dot_over_theta = if_then_else(
337
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 LT, t, TaylorSeriesExpansion<Scalar>::template precision<3>(),
338 ✗ static_cast<Scalar>(Scalar(1) / Scalar(360)), // then
339 ✗ static_cast<Scalar>(
340
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 -Scalar(2) * t2inv * t2inv + (Scalar(1) + st * tinv) * t2inv * inv_2_2ct) // else
341 );
342
343
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 const Scalar wTp = w.dot(p);
344
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 124 const Vector3 v3_tmp(
345
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 124 (beta_dot_over_theta * wTp) * w - (t2 * beta_dot_over_theta + Scalar(2) * beta) * p);
346 // C can be treated as a temporary variable
347
4/8
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 121 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 121 times.
✗ Branch 11 not taken.
 124 C.noalias() = v3_tmp * w.transpose();
348
5/10
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 121 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 121 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 121 times.
✗ Branch 14 not taken.
 124 C.noalias() += beta * w * p.transpose();
349
3/6
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 121 times.
✗ Branch 8 not taken.
 124 C.diagonal().array() += wTp * beta;
350
2/4
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
 124 addSkew(Scalar(.5) * p, C);
351
352
3/6
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 121 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 121 times.
✗ Branch 8 not taken.
 124 B.noalias() = C * A;
353
1/2
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 124 C.setZero();
354 124 }
355 };
356
357 } // namespace pinocchio
358
359 #endif // ifndef __pinocchio_spatial_log_hxx__
360