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