5 template<WorldFrame::WorldFrame FRAME>
7 float Ax,
float Ay,
float Az,
8 float Mx,
float My,
float Mz,
9 double& q0,
double& q1,
double& q2,
double& q3) {
11 geometry_msgs::Vector3 A, M;
12 geometry_msgs::Quaternion orientation;
13 A.x = Ax; A.y = Ay; A.z = Az;
14 M.x = Mx; M.y = My; M.z = Mz;
26 template<WorldFrame::WorldFrame FRAME>
28 float Ax,
float Ay,
float Az,
29 double& q0,
double& q1,
double& q2,
double& q3) {
31 geometry_msgs::Vector3 A;
32 geometry_msgs::Quaternion orientation;
33 A.x = Ax; A.y = Ay; A.z = Az;
46 #define TEST_STATELESS_ENU(in_am, exp_result) \ 47 TEST(StatelessOrientationTest, Stationary_ENU_ ## in_am){ \ 48 double q0, q1, q2, q3; \ 49 ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(in_am, q0, q1, q2, q3)); \ 50 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 51 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 52 TEST(StatelessOrientationTest, Stationary_ENU_NM_ ## in_am){ \ 53 double q0, q1, q2, q3; \ 54 ASSERT_TRUE(computeOrientation<WorldFrame::ENU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 55 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 56 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 58 #define TEST_STATELESS_NED(in_am, exp_result) \ 59 TEST(StatelessOrientationTest, Stationary_NED_ ## in_am){ \ 60 double q0, q1, q2, q3; \ 61 ASSERT_TRUE(computeOrientation<WorldFrame::NED>(in_am, q0, q1, q2, q3)); \ 62 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 63 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 64 TEST(StatelessOrientationTest, Stationary_NED_NM_ ## in_am){ \ 65 double q0, q1, q2, q3; \ 66 ASSERT_TRUE(computeOrientation<WorldFrame::NED>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 67 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 68 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 70 #define TEST_STATELESS_NWU(in_am, exp_result) \ 71 TEST(StatelessOrientationTest, Stationary_NWU_ ## in_am){ \ 72 double q0, q1, q2, q3; \ 73 ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(in_am, q0, q1, q2, q3)); \ 74 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 75 ASSERT_QUAT_EQUAL(q0, q1, q2, q3, exp_result); } \ 76 TEST(StatelessOrientationTest, Stationary_NWU_NM_ ## in_am){ \ 77 double q0, q1, q2, q3; \ 78 ASSERT_TRUE(computeOrientation<WorldFrame::NWU>(ACCEL_ONLY(in_am), q0, q1, q2, q3)); \ 79 ASSERT_IS_NORMALIZED(q0, q1, q2, q3); \ 80 ASSERT_QUAT_EQUAL_EX_Z(q0, q1, q2, q3, exp_result); } 101 TEST(StatelessOrientationTest, Check_NoAccel){
102 double q0, q1, q2, q3;
103 ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
105 0.0, 0.0005, -0.0005,
109 TEST(StatelessOrientationTest, Check_NoMag){
110 double q0, q1, q2, q3;
111 ASSERT_FALSE(computeOrientation<WorldFrame::ENU>(
#define QUAT_WEST_NORTH_DOWN_RSD_ENU
#define QUAT_NE_NW_UP_RSD_ENU
#define QUAT_WEST_NORTH_DOWN_RSD_NED
#define TEST_STATELESS_NWU(in_am, exp_result)
TEST(StatelessOrientationTest, Check_NoAccel)
#define AM_NORTH_EAST_DOWN
#define AM_WEST_NORTH_DOWN
#define TEST_STATELESS_ENU(in_am, exp_result)
#define AM_WEST_NORTH_DOWN_RSD
#define QUAT_NE_NW_UP_RSD_NWU
#define TEST_STATELESS_NED(in_am, exp_result)
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
#define QUAT_NE_NW_UP_RSD_NED
#define QUAT_WEST_NORTH_DOWN_RSD_NWU
bool computeOrientation(float Ax, float Ay, float Az, float Mx, float My, float Mz, double &q0, double &q1, double &q2, double &q3)