test_helpers.h
Go to the documentation of this file.
1 
2 #ifndef TEST_TEST_HELPERS_H_
3 #define TEST_TEST_HELPERS_H_
4 
5 #include <gtest/gtest.h>
7 
8 #define MAX_DIFF 0.05
9 
10 template <typename T>
11 static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) {
12  T invNorm = 1 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
13  T max = q0;
14  if (fabs(max) < fabs(q1)) max = q1;
15  if (fabs(max) < fabs(q2)) max = q2;
16  if (fabs(max) < fabs(q3)) max = q3;
17  if (max < 0) invNorm *= -1.0;
18 
19  q0 *= invNorm;
20  q1 *= invNorm;
21  q2 *= invNorm;
22  q3 *= invNorm;
23 }
24 
25 // Tests for normalization in the same way as tf2:
26 // https://github.com/ros/geometry2/blob/bd490515b1434caeff521ea14901dfe04063ca27/tf2/src/buffer_core.cpp#L244-L247
27 template <typename T>
28 static inline bool is_normalized(T q0, T q1, T q2, T q3) {
29  return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6;
30 }
31 
32 template <typename T>
33 static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
34  normalize_quaternion(q0, q1, q2, q3);
35  normalize_quaternion(qr0, qr1, qr2, qr3);
36 
37  return (fabs(q0 - qr0) < MAX_DIFF) &&
38  (fabs(q1 - qr1) < MAX_DIFF) &&
39  (fabs(q2 - qr2) < MAX_DIFF) &&
40  (fabs(q3 - qr3) < MAX_DIFF);
41 }
42 
43 template <typename T>
44 static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
45  // assert q == qr * qz
46  tf2::Quaternion q(q1, q2, q3, q0);
47  tf2::Quaternion qr(qr1, qr2, qr3, qr0);
48  tf2::Quaternion qz = q * qr.inverse();
49 
50  // remove x and y components.
51  qz.setX(0.0);
52  qz.setY(0.0);
53 
54  tf2::Quaternion qr_ = qz * qr;
55 
56  return quat_equal(q0, q1, q2, q3,
57  qr_.getW(),
58  qr_.getX(),
59  qr_.getY(),
60  qr_.getZ());
61 }
62 
63 #define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
64 #define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__)
65 
66 #define ASSERT_QUAT_EQUAL_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_equal(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
67 #define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__)
68 
69 #define ASSERT_QUAT_EQUAL_EX_Z_(q0, q1, q2, q3, qr0, qr1, qr2, qr3) ASSERT_TRUE(quat_eq_ex_z(q0, q1, q2, q3, qr0, qr1, qr2, qr3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
70 #define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__)
71 
72 // Well known states
73 // scheme: AM_x_y_z
74 #define AM_EAST_NORTH_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ 0.0, 0.0005, -0.0005
75 #define AM_NORTH_EAST_DOWN /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0005, 0.0, 0.0005
76 #define AM_NORTH_WEST_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ 0.0005, 0.0, -0.0005
77 #define AM_SOUTH_UP_WEST /* Acceleration */ 0.0, 9.81, 0.0, /* Magnetic */ -0.0005, -0.0005, 0.0
78 #define AM_SOUTH_EAST_UP /* Acceleration */ 0.0, 0.0, 9.81, /* Magnetic */ -0.0005, 0.0, -0.0005
79 #define AM_WEST_NORTH_DOWN /* Acceleration */ 0.0, 0.0, -9.81, /* Magnetic */ 0.0, 0.0005, 0.0005
80 
81 // Real sensor data
82 #define AM_WEST_NORTH_DOWN_RSD /* Acceleration */ 0.12, 0.29, -9.83, /* Magnetic */ 6.363e-06, 1.0908e-05, 4.2723e-05
83 #define AM_NE_NW_UP_RSD /* Acceleration */ 0.20, 0.55, 9.779, /* Magnetic */ 8.484e-06, 8.181e-06, -4.3329e-05
84 
85 // Strip accelration from am
86 #define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az
87 
88 // Well known quaternion
89 // scheme: QUAT_axis_angle
90 #define QUAT_IDENTITY 1.0, 0.0, 0.0, 0.0
91 #define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107
92 #define QUAT_X_180 0.0, 1.0, 0.0, 0.0
93 #define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5
94 #define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012 /* axis: (0.864401, 0.502559, 0.0120614) | angle: 178.848deg */
95 #define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02/* Axis: (-0.2, -0.96, 0.02), Angle: 180deg */
96 #define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50 /* Axis: -Z, Angle: 60deg */
97 #define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41 /* axis: (0.0300376, -0.020025, -0.410513) | angle: 48.6734deg */
98 #define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35 /* Axis: Z, Angle: 41deg */
99 #define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02 /* Axis: (0.9, 0.4, 0.0), Angle: 180deg */
100 
101 
102 #endif /* TEST_TEST_HELPERS_H_ */
f
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
Quaternion inverse() const
static bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3)
Definition: test_helpers.h:33
#define MAX_DIFF
Definition: test_helpers.h:8
static bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3)
Definition: test_helpers.h:44
static bool is_normalized(T q0, T q1, T q2, T q3)
Definition: test_helpers.h:28
static void normalize_quaternion(T &q0, T &q1, T &q2, T &q3)
Definition: test_helpers.h:11
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
double max(double a, double b)


imu_filter_madgwick
Author(s): Ivan Dryanovski
autogenerated on Thu Jun 18 2020 03:40:00