void test_default_values(const CPose3D &p, const std::string & label) { EXPECT_EQ(p.x(),0); EXPECT_EQ(p.y(),0); EXPECT_EQ(p.z(),0); EXPECT_EQ(p.yaw(),0); EXPECT_EQ(p.pitch(),0); EXPECT_EQ(p.roll(),0); for (size_t i=0;i<4;i++) for (size_t j=0;j<4;j++) EXPECT_NEAR(p.getHomogeneousMatrixVal()(i,j), i==j ? 1.0 : 0.0, 1e-8 ) << "Failed for (i,j)=" << i << "," << j << endl << "Matrix is: " << endl << p.getHomogeneousMatrixVal() << endl << "case was: " << label << endl; }
void test_compose(double x1,double y1,double z1, double yaw1,double pitch1,double roll1, double x2,double y2,double z2, double yaw2,double pitch2,double roll2 ) { const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1); const CPose3D p2(x2,y2,z2,yaw2,pitch2,roll2); const CPose3D p1_c_p2 = p1 + p2; const CPose3D p1_i_p2 = p1 - p2; const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1; // should be -> p2 const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2; // Should be -> p1 EXPECT_NEAR(0, (p1_c_p2_i_p2.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(), 1e-5) << "p2 : " << p2 << endl << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl; EXPECT_NEAR(0, (p2_c_p1_i_p2.getAsVectorVal()-p1.getAsVectorVal()).array().abs().sum(), 1e-5) << "p1 : " << p1 << endl << "p2 : " << p2 << endl << "p2 matrix : " << endl << p2.getHomogeneousMatrixVal() << endl << "p1_i_p2 : " << p1_i_p2 << endl << "p1_i_p2 matrix: " << endl << p1_i_p2.getHomogeneousMatrixVal() << endl << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl; }
// Some TEST(SLERP_tests, correctShortestPath) { { // Both poses at (0,0,0) angles: const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0)); { CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0,pose_interp); const CPose3D expected(0,0,0,0,0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,1,pose_interp); const CPose3D expected(0,0,0,0,0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,0,0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } } { // Poses at yaw=+-179deg const CPose3D pose_a(0,0,0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(-180),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at yaw=-+179deg const CPose3D pose_a(0,0,0, DEG2RAD(-179),DEG2RAD(0),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(179),DEG2RAD(0),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(-180),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at yaw=+-40 const CPose3D pose_a(0,0,0, DEG2RAD(40),DEG2RAD(0),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(-40),DEG2RAD(0),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at yaw=-+40 const CPose3D pose_a(0,0,0, DEG2RAD(-40),DEG2RAD(0),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(40),DEG2RAD(0),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at pitch=+-40 const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD( 40),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(-40),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at pitch=-+40 const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(-40),DEG2RAD(0)); const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD( 40),DEG2RAD(0)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at roll=-+40 const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(-40)); const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD( 40)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } { // Poses at roll=+-40 const CPose3D pose_a(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD( 40)); const CPose3D pose_b(0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(-40)); CPose3D pose_interp; mrpt::math::slerp(pose_a,pose_b,0.5,pose_interp); const CPose3D expected(0,0,0,DEG2RAD(0),0,0); EXPECT_NEAR(0, (pose_interp.getHomogeneousMatrixVal()-expected.getHomogeneousMatrixVal()).array().abs().sum(), 1e-4 ) << "pose_a: " << pose_a << "\npose_b: " << pose_b << "\ninterp: " << pose_interp << endl; } }