Exemplo n.º 1
0
void leatherman::getRPY(const Eigen::Matrix3d &m, double &roll, double &pitch, double &yaw)
{
  Eigen::Vector3d v = m.eulerAngles(0,1,2);
  roll = v(0);  pitch = v(1);  yaw = v(2);
}