/** Constructor from a CPose3D */
CPose3DQuat::CPose3DQuat(const CPose3D &p)
{
x() = p.x();
y() = p.y();
z() = p.z();
p.getAsQuaternion(m_quat);
}
void mrpt::math::slerp(
const CPose3D & p0,
const CPose3D & p1,
const double t,
CPose3D & p)
{
CQuaternionDouble q0,q1,q;
p0.getAsQuaternion(q0);
p1.getAsQuaternion(q1);
// The quaternion part (this will raise exception on t not in [0,1])
mrpt::math::slerp(q0,q1,t, q);
// XYZ:
p = CPose3D(
q,
(1-t)*p0.x()+t*p1.x(),
(1-t)*p0.y()+t*p1.y(),
(1-t)*p0.z()+t*p1.z() );
}
void test_lnAndExpMatches(double yaw1,double pitch1,double roll1)
{
const CPose3D pp(0,0,0,yaw1,pitch1,roll1);
CQuaternionDouble q1;
pp.getAsQuaternion(q1);
mrpt::vector_double q1_ln = q1.ln<mrpt::vector_double>();
const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln);
// q2 should be == q1
EXPECT_NEAR(0, (q1-q2).Abs().sumAll(), 1e-10 )
<< "q1:\n" << q1 << endl
<< "q2:\n" << q2 << endl
<< "Error:\n" << (q1-q2) << endl;
}
