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;
}
}
