TEST_F(QuaternionTests, crossProduct)
{
CQuaternionDouble q1, q2, q3;
// q1 = CQuaternionDouble(1,2,3,4); q1.normalize();
CPose3D p1(0, 0, 0, DEG2RAD(10), DEG2RAD(30), DEG2RAD(-20));
p1.getAsQuaternion(q1);
CPose3D p2(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(10));
p2.getAsQuaternion(q2);
// q3 = q1 x q2
q3.crossProduct(q1, q2);
const CPose3D p3 = p1 + p2;
EXPECT_NEAR(
0, std::abs(
(p3.getAsVectorVal() - CPose3D(q3, 0, 0, 0).getAsVectorVal())
.sum()),
1e-6)
<< "q1 = " << q1 << endl
<< "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl
<< "q2 = " << q2 << endl
<< "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl
<< "q3 = q1 * q2 = " << q3 << endl
<< "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl
<< "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
}
void test_ExpLnEqual(double x1,double y1,double z1, double yaw1,double pitch1,double roll1)
{
const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
const CPose3D p2 = CPose3D::exp( p1.ln() );
EXPECT_NEAR((p1.getAsVectorVal()-p2.getAsVectorVal()).array().abs().sum(),0, 1e-5 ) << "p1: " << p1 <<endl;
}
void test_composeFrom(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_plus_p2 = p1 + p2;
{
CPose3D p1_plus_p2bis;
p1_plus_p2bis.composeFrom(p1,p2);
EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
<< "p2 : " << p2 << endl
<< "p1 : " << p1 << endl
<< "p1_plus_p2 : " << p1_plus_p2 << endl
<< "p1_plus_p2bis : " << p1_plus_p2bis<< endl;
}
{
CPose3D p1_plus_p2bis = p1;
p1_plus_p2bis.composeFrom(p1_plus_p2bis,p2);
EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
<< "p2 : " << p2 << endl
<< "p1 : " << p1 << endl
<< "p1_plus_p2 : " << p1_plus_p2 << endl
<< "p1_plus_p2bis : " << p1_plus_p2bis<< endl;
}
{
CPose3D p1_plus_p2bis = p2;
p1_plus_p2bis.composeFrom(p1,p1_plus_p2bis);
EXPECT_NEAR(0, (p1_plus_p2bis.getAsVectorVal()-p1_plus_p2.getAsVectorVal()).array().abs().sum(), 1e-5)
<< "p2 : " << p2 << endl
<< "p1 : " << p1 << endl
<< "p1_plus_p2 : " << p1_plus_p2 << endl
<< "p1_plus_p2bis : " << p1_plus_p2bis<< 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;
}
