Eigen::Quaterniond quaternion(pose.orientation.w,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z);
Eigen::Vector3d eulerAngle = quaternion.matrix().eulerAngles(2,1,0);
pose.position.z = eulerAngle[0] * 180.0 / 3.1415926;
Eigen 四元数转欧拉角
最新推荐文章于 2024-04-16 19:59:55 发布