Eigen 四元数转欧拉角

  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;