Skip to content

Commit 43c5833

Browse files
giafranchinigiafranchini
committed
test quaternionToAngleAxis for very small rotations
1 parent b21c169 commit 43c5833

File tree

2 files changed

+183
-106
lines changed

2 files changed

+183
-106
lines changed

‎fuse_core/include/fuse_core/util.hpp‎

Lines changed: 100 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@
3939
#include <Eigen/Core>
4040

4141
#include <cmath>
42+
#include <numeric>
4243
#include <string>
4344

4445
#include <rclcpp/logging.hpp>
@@ -158,7 +159,7 @@ Eigen::Matrix<T, 2, 2, Eigen::RowMajor> rotationMatrix2D(const T angle)
158159
* @param[in] rpy Pointer to the roll, pitch, yaw array (3x1)
159160
* @param[in] jacobian Pointer to the jacobian matrix (3x4, optional)
160161
*/
161-
static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr)
162+
static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr)
162163
{
163164
rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]);
164165
rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]);
@@ -170,8 +171,8 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob
170171
const double qx = q[1];
171172
const double qy = q[2];
172173
const double qz = q[3];
173-
const double discr = qw * qy - qx * qz;
174-
const double gl_limit = 0.5 - 2.0 * std::numeric_limits<double>::epsilon();
174+
const double discr = qw * qy - qx * qz;
175+
const double gl_limit = 0.5 - 2.0 * std::numeric_limits<double>::epsilon();
175176

176177
if (discr > gl_limit) {
177178
// pitch = 90 deg
@@ -189,82 +190,114 @@ static inline void quaternion2rpy(const double * q, double * rpy, double * jacob
189190
// Non-degenerate case:
190191
jacobian_map(0, 0) =
191192
-(2.0 * qx) /
192-
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
193+
((std::pow(
194+
(2.0 * qw * qx + 2.0 * qy * qz),
195+
2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
193196
1.0) *
194197
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));
195198
jacobian_map(0, 1) =
196199
-((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
197-
(4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
198-
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
200+
(4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) /
201+
std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
202+
(std::pow(
203+
(2.0 * qw * qx + 2.0 * qy * qz),
204+
2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
199205
jacobian_map(0, 2) =
200206
-((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) -
201-
(4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
202-
(std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
207+
(4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) /
208+
std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) /
209+
(std::pow(
210+
(2.0 * qw * qx + 2.0 * qy * qz),
211+
2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0);
203212
jacobian_map(0, 3) =
204213
-(2.0 * qy) /
205-
((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
214+
((std::pow(
215+
(2.0 * qw * qx + 2.0 * qy * qz),
216+
2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) +
206217
1.0) *
207218
(2.0 * qx * qx + 2.0 * qy * qy - 1.0));
208219

209-
jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
210-
jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
211-
jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
212-
jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
220+
jacobian_map(
221+
1,
222+
0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
223+
jacobian_map(
224+
1,
225+
1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
226+
jacobian_map(
227+
1,
228+
2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
229+
jacobian_map(
230+
1,
231+
3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0));
213232

214233
jacobian_map(2, 0) =
215234
-(2.0 * qz) /
216-
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
235+
((std::pow(
236+
(2.0 * qw * qz + 2.0 * qx * qy),
237+
2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
217238
1.0) *
218239
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
219240
jacobian_map(2, 1) =
220241
-(2.0 * qy) /
221-
((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
242+
((std::pow(
243+
(2.0 * qw * qz + 2.0 * qx * qy),
244+
2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) +
222245
1.0) *
223246
(2.0 * qy * qy + 2.0 * qz * qz - 1.0));
224247
jacobian_map(2, 2) =
225248
-((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
226-
(4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
227-
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
249+
(4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) /
250+
std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
251+
(std::pow(
252+
(2.0 * qw * qz + 2.0 * qx * qy),
253+
2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
228254
jacobian_map(2, 3) =
229255
-((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) -
230-
(4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
231-
(std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
256+
(4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) /
257+
std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) /
258+
(std::pow(
259+
(2.0 * qw * qz + 2.0 * qx * qy),
260+
2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0);
232261
}
233262
}
234263
}
235264

236265
/**
237266
* @brief Compute product of two quaternions and the function jacobian
238-
* TODO(giafranchini): parametric jacobian computation? Atm this function is only used in
267+
* TODO(giafranchini): parametric jacobian computation? Atm this function is only used in
239268
* normal_prior_pose_3d cost function. There we only need the derivatives wrt quaternion W,
240269
* so at the time we are only computing the jacobian wrt W
241-
*
270+
*
242271
* @param[in] z Pointer to the first quaternion array (4x1)
243272
* @param[in] w Pointer to the second quaternion array (4x1)
244273
* @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w
245274
* @param[in] jacobian Pointer to the jacobian of zw with respect to w (4x4, optional)
246275
*/
247-
static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr)
276+
static inline void quaternionProduct(
277+
const double * z, const double * w, double * zw,
278+
double * jacobian = nullptr)
248279
{
249280
ceres::QuaternionProduct(z, w, zw);
250281
if (jacobian) {
251282
Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>> jacobian_map(jacobian);
252-
jacobian_map <<
283+
jacobian_map <<
253284
z[0], -z[1], -z[2], -z[3],
254-
z[1], z[0], -z[3], z[2],
255-
z[2], z[3], z[0], -z[1],
256-
z[3], -z[2], z[1], z[0];
285+
z[1], z[0], -z[3], z[2],
286+
z[2], z[3], z[0], -z[1],
287+
z[3], -z[2], z[1], z[0];
257288
}
258289
}
259290

260291
/**
261292
* @brief Compute quaternion to AngleAxis conversion and the function jacobian
262-
*
293+
*
263294
* @param[in] q Pointer to the quaternion array (4x1)
264-
* @param[in] angle_axis Pointer to the angle_axis array (3x1)
295+
* @param[in] angle_axis Pointer to the angle_axis array (3x1)
265296
* @param[in] jacobian Pointer to the jacobian matrix (3x4, optional)
266297
*/
267-
static inline void quaternionToAngleAxis(const double * q, double * angle_axis, double * jacobian = nullptr)
298+
static inline void quaternionToAngleAxis(
299+
const double * q, double * angle_axis,
300+
double * jacobian = nullptr)
268301
{
269302
ceres::QuaternionToAngleAxis(q, angle_axis);
270303
if (jacobian) {
@@ -275,47 +308,51 @@ static inline void quaternionToAngleAxis(const double * q, double * angle_axis,
275308
const double & q3 = q[3];
276309
const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3;
277310
const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3;
278-
const double sin_theta = std::sqrt(sin_theta2);
311+
const double sin_theta = std::hypot(q1, q2, q3);
279312
const double cos_theta = q0;
280313

281-
if (std::fpclassify(sin_theta) != FP_ZERO) {
282-
const double two_theta = 2.0 *
283-
(cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta));
314+
const double cond = std::pow(sin_theta2, 1.5);
315+
if (std::fpclassify(cond) != FP_ZERO) {
316+
const double two_theta = 2.0 *
317+
(cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta));
318+
const double a = two_theta / sin_theta;
319+
const double b = sin_theta2 * q_sum2;
320+
const double c = two_theta / cond;
284321
jacobian_map(0, 0) = -2.0 * q1 / q_sum2;
285-
jacobian_map(0, 1) =
286-
two_theta / sin_theta +
287-
(2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) -
288-
(q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5);
289-
jacobian_map(0, 2) =
290-
(2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) -
291-
(q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5);
322+
jacobian_map(0, 1) =
323+
two_theta / sin_theta +
324+
(2.0 * q0 * q1 * q1) / b -
325+
(q1 * q1 * c);
326+
jacobian_map(0, 2) =
327+
(2.0 * q0 * q1 * q2) / b -
328+
(q1 * q2 * c);
292329
jacobian_map(0, 3) =
293-
(2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) -
294-
(q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5);
295-
330+
(2.0 * q0 * q1 * q3) / b -
331+
(q1 * q3 * c);
332+
296333
jacobian_map(1, 0) = -2.0 * q2 / q_sum2;
297-
jacobian_map(1, 1) =
298-
(2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) -
299-
(q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5);
300-
jacobian_map(1, 2) =
301-
two_theta / sin_theta +
302-
(2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) -
303-
(q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5);
304-
jacobian_map(1, 3) =
305-
(2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) -
306-
(q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5);
307-
334+
jacobian_map(1, 1) =
335+
(2.0 * q0 * q1 * q2) / b -
336+
(q1 * q2 * c);
337+
jacobian_map(1, 2) =
338+
two_theta / sin_theta +
339+
(2.0 * q0 * q2 * q2) / b -
340+
(q2 * q2 * c);
341+
jacobian_map(1, 3) =
342+
(2.0 * q0 * q2 * q3) / b -
343+
(q2 * q3 * c);
344+
308345
jacobian_map(2, 0) = -2.0 * q3 / q_sum2;
309-
jacobian_map(2, 1) =
310-
(2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) -
311-
(q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5);
312-
jacobian_map(2, 2) =
313-
(2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) -
314-
(q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5);
346+
jacobian_map(2, 1) =
347+
(2.0 * q0 * q1 * q3) / b -
348+
(q1 * q3 * c);
349+
jacobian_map(2, 2) =
350+
(2.0 * q0 * q2 * q3) / b -
351+
(q2 * q3 * c);
315352
jacobian_map(2, 3) =
316-
two_theta / sin_theta +
317-
(2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) -
318-
(q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5);
353+
two_theta / sin_theta +
354+
(2.0 * q0 * q3 * q3) / b -
355+
(q3 * q3 * c);
319356
} else {
320357
jacobian_map.setZero();
321358
jacobian_map(0, 1) = 2.0;

0 commit comments

Comments
 (0)