00001
00002 #ifndef TEST_TEST_HELPERS_H_
00003 #define TEST_TEST_HELPERS_H_
00004
00005 #include <gtest/gtest.h>
00006 #include <tf2/LinearMath/Quaternion.h>
00007
00008 #define MAX_DIFF 0.05
00009
00010 template <typename T>
00011 static inline void normalize_quaternion(T& q0, T& q1, T& q2, T& q3) {
00012 T invNorm = 1 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
00013 T max = q0;
00014 if (fabs(max) < fabs(q1)) max = q1;
00015 if (fabs(max) < fabs(q2)) max = q2;
00016 if (fabs(max) < fabs(q3)) max = q3;
00017 if (max < 0) invNorm *= -1.0;
00018
00019 q0 *= invNorm;
00020 q1 *= invNorm;
00021 q2 *= invNorm;
00022 q3 *= invNorm;
00023 }
00024
00025
00026
00027 template <typename T>
00028 static inline bool is_normalized(T q0, T q1, T q2, T q3) {
00029 return std::abs((q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3) - 1.0f) < 10e-6;
00030 }
00031
00032 template <typename T>
00033 static inline bool quat_equal(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
00034 normalize_quaternion(q0, q1, q2, q3);
00035 normalize_quaternion(qr0, qr1, qr2, qr3);
00036
00037 return (fabs(q0 - qr0) < MAX_DIFF) &&
00038 (fabs(q1 - qr1) < MAX_DIFF) &&
00039 (fabs(q2 - qr2) < MAX_DIFF) &&
00040 (fabs(q3 - qr3) < MAX_DIFF);
00041 }
00042
00043 template <typename T>
00044 static inline bool quat_eq_ex_z(T q0, T q1, T q2, T q3, T qr0, T qr1, T qr2, T qr3) {
00045
00046 tf2::Quaternion q(q1, q2, q3, q0);
00047 tf2::Quaternion qr(qr1, qr2, qr3, qr0);
00048 tf2::Quaternion qz = q * qr.inverse();
00049
00050
00051 qz.setX(0.0);
00052 qz.setY(0.0);
00053
00054 tf2::Quaternion qr_ = qz * qr;
00055
00056 return quat_equal(q0, q1, q2, q3,
00057 qr_.getW(),
00058 qr_.getX(),
00059 qr_.getY(),
00060 qr_.getZ());
00061 }
00062
00063 #define ASSERT_IS_NORMALIZED_(q0, q1, q2, q3) ASSERT_TRUE(is_normalized(q0, q1, q2, q3)) << "q0: " << q0 << ", q1: " << q1 << ", q2: " << q2 << ", q3: " << q3;
00064 #define ASSERT_IS_NORMALIZED(...) ASSERT_IS_NORMALIZED_(__VA_ARGS__)
00065
00066 #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;
00067 #define ASSERT_QUAT_EQUAL(...) ASSERT_QUAT_EQUAL_(__VA_ARGS__)
00068
00069 #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;
00070 #define ASSERT_QUAT_EQUAL_EX_Z(...) ASSERT_QUAT_EQUAL_EX_Z_(__VA_ARGS__)
00071
00072
00073
00074 #define AM_EAST_NORTH_UP 0.0, 0.0, 9.81, 0.0, 0.0005, -0.0005
00075 #define AM_NORTH_EAST_DOWN 0.0, 0.0, -9.81, 0.0005, 0.0, 0.0005
00076 #define AM_NORTH_WEST_UP 0.0, 0.0, 9.81, 0.0005, 0.0, -0.0005
00077 #define AM_SOUTH_UP_WEST 0.0, 9.81, 0.0, -0.0005, -0.0005, 0.0
00078 #define AM_SOUTH_EAST_UP 0.0, 0.0, 9.81, -0.0005, 0.0, -0.0005
00079 #define AM_WEST_NORTH_DOWN 0.0, 0.0, -9.81, 0.0, 0.0005, 0.0005
00080
00081
00082 #define AM_WEST_NORTH_DOWN_RSD 0.12, 0.29, -9.83, 6.363e-06, 1.0908e-05, 4.2723e-05
00083 #define AM_NE_NW_UP_RSD 0.20, 0.55, 9.779, 8.484e-06, 8.181e-06, -4.3329e-05
00084
00085
00086 #define ACCEL_ONLY(ax,ay,az, mx,my,mz) ax, ay, az
00087
00088
00089
00090 #define QUAT_IDENTITY 1.0, 0.0, 0.0, 0.0
00091 #define QUAT_MZ_90 0.707107, 0.0, 0.0, -0.707107
00092 #define QUAT_X_180 0.0, 1.0, 0.0, 0.0
00093 #define QUAT_XMYMZ_120 0.5, 0.5, -0.5, -0.5
00094 #define QUAT_WEST_NORTH_DOWN_RSD_NWU 0.01, 0.86, 0.50, 0.012
00095 #define QUAT_WEST_NORTH_DOWN_RSD_ENU 0.0, -0.25, -0.97, -0.02
00096 #define QUAT_WEST_NORTH_DOWN_RSD_NED 0.86, -0.01, 0.01, -0.50
00097 #define QUAT_NE_NW_UP_RSD_NWU 0.91, 0.03, -0.02, -0.41
00098 #define QUAT_NE_NW_UP_RSD_ENU 0.93, 0.03, 0.0, 0.35
00099 #define QUAT_NE_NW_UP_RSD_NED 0.021, -0.91, -0.41, 0.02
00100
00101
00102 #endif