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