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 |