ROS x y z w四元素转欧拉角 roll pitch yaw C++实现

ROS roll pitch yaw 欧拉角转四元素 C++实现

void GetRPYFromXYZW(double x, double y, double z, double w, double& roll, double& pitch, double& yaw)
{
  roll = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
  pitch = asin(2 * (w * y - z * x));
  yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
}

x y z w 对应四元素 ， roll pitch yaw 返回欧拉角