// ------------------------------------------------------
// Generate both sets of points
// ------------------------------------------------------
CPose3DQuat generate_points( TPoints &pA, TPoints &pB )
{
const double Dx = 0.5;
const double Dy = 1.5;
const double Dz = 0.75;
const double yaw = DEG2RAD(10);
const double pitch = DEG2RAD(20);
const double roll = DEG2RAD(5);
pA.resize( 5 ); // A set of points at "A" reference system
pB.resize( 5 ); // A set of points at "B" reference system
pA[0].resize(3); pA[0][0] = 0.0; pA[0][1] = 0.5; pA[0][2] = 0.4;
pA[1].resize(3); pA[1][0] = 1.0; pA[1][1] = 1.5; pA[1][2] = -0.1;
pA[2].resize(3); pA[2][0] = 1.2; pA[2][1] = 1.1; pA[2][2] = 0.9;
pA[3].resize(3); pA[3][0] = 0.7; pA[3][1] = 0.3; pA[3][2] = 3.4;
pA[4].resize(3); pA[4][0] = 1.9; pA[4][1] = 2.5; pA[4][2] = -1.7;
CPose3DQuat qPose = CPose3DQuat(CPose3D( Dx, Dy, Dz, yaw, pitch, roll ));
for( unsigned int i = 0; i < 5; ++i )
{
pB[i].resize( 3 );
qPose.inverseComposePoint( pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2] );
}
return qPose;
} // end generate_points
TPoint3D mrpt::poses::operator-(const TPoint3D& G, const CPose3DQuat& p)
{
mrpt::math::TPoint3D L;
p.inverseComposePoint(G[0], G[1], G[2], L[0], L[1], L[2]);
return L;
}