C++ Python使用常用库时如何做欧拉角 ⇄ 四元数转换

下面在原有推导与基础代码的基础上，专门补充使用常用库时如何做欧拉角 ⇄ 四元数转换，分别针对：

• C++：Eigen、ROS tf2、GLM（游戏/图形常用）
• Python：SciPy、ROS tf_transformations、NumPy-Quaternion（可选）

仍然使用上一版的约定（如无特别说明）：

• 右手坐标系
• 欧拉角顺序：Z-Y-X（yaw-pitch-roll）
• 欧拉角含义：
  • roll：绕 x 轴
  • pitch：绕 y 轴
  • yaw：绕 z 轴
• 四元数：((w, x, y, z))，单位四元数

非常关键：不同库对欧拉顺序和四元数分量顺序的约定不完全一样，下面会在每个库的示例里明确写清楚，避免"方向反了""绕错轴"等问题。

---

一、C++ 中用常见库进行转换

1.1 使用 Eigen（C++ 线性代数库）

Eigen 在机器人学和图形学领域非常常用。

核心类型：

• Eigen::Quaterniond：四元数，内部存储顺序为 (x, y, z, w)
• Eigen::Matrix3d：3x3 旋转矩阵
• Eigen::AngleAxisd 和 Eigen::Transform 也可以描述旋转

Eigen 没有直接"给 roll, pitch, yaw 就生成四元数"的单函数 ，但可以通过 AngleAxis 或 eulerAngles 组合。

1.1.1 欧拉角 → 四元数（Eigen，ZYX）

想要 ZYX（yaw, pitch, roll），可以通过旋转矩阵中间步骤实现：

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <iostream>

// 欧拉角 -> 四元数
// 输入：roll, pitch, yaw（弧度）
// 旋转顺序：Z-Y-X（yaw-pitch-roll）
Eigen::Quaterniond euler_to_quaternion_eigen(double roll, double pitch, double yaw)
{
    // 按 Z-Y-X 生成旋转矩阵
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());

    // 从旋转矩阵生成四元数（内部为 (x,y,z,w)）
    Eigen::Quaterniond q(R);
    q.normalize();   // 以防数值误差
    return q;
}

Eigen::Quaterniond 的 coeffs() 顺序是 (x, y, z, w)，如果你需要 (w, x, y, z)，要手动取：

Eigen::Quaterniond q = euler_to_quaternion_eigen(roll, pitch, yaw);
double w = q.w();
double x = q.x();
double y = q.y();
double z = q.z();

1.1.2 四元数 → 欧拉角（Eigen，ZYX）

Eigen 提供 matrix().eulerAngles(0,1,2) 等方法从旋转矩阵中提取欧拉角，不过索引顺序比较绕 ，所以推荐一种更直观的形式：直接用 RotationMatrix + eulerAngles(2,1,0) 表示 ZYX。

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <tuple>

// 四元数 -> 欧拉角 (roll, pitch, yaw)
std::tuple<double, double, double> quaternion_to_euler_eigen(const Eigen::Quaterniond& q)
{
    // 转为旋转矩阵
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();

    // eulerAngles 的参数是轴索引：0->X, 1->Y, 2->Z
    // Z-Y-X（yaw, pitch, roll），因此使用 (2,1,0)
    Eigen::Vector3d euler = R.eulerAngles(2, 1, 0);

    double yaw   = euler[0];
    double pitch = euler[1];
    double roll  = euler[2];

    return std::make_tuple(roll, pitch, yaw);
}

如果你要用 X-Y-Z 顺序，只需要对应改成 R.eulerAngles(0,1,2) 等即可，但要与自己定义的数学公式对齐。

---

1.2 使用 ROS tf2（ROS 机器人框架）

在 ROS1 中常见的是 tf；在 ROS2 或新代码中推荐 tf2 和 tf2_geometry_msgs。

常用类型：

• tf2::Quaternion：四元数（内部是 (x, y, z, w)）
• tf2::Matrix3x3：3x3 旋转矩阵风格的类

1.2.1 欧拉角 → 四元数（tf2）

tf2::Quaternion 提供了 setRPY(roll, pitch, yaw) 方法，顺序为：R-P-Y，对应 X-Y-Z ，即默认是 X-Y-Z 内部实现顺序为 yaw-pitch-roll？

但从使用角度看，你只要把 roll, pitch, yaw 直接填进去即可，tf2 会按其内部的一套约定生成姿态。关键是"正反变换要用同一套 tf2 函数"，不混用别的库的 ZYX 公式即可。

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

// 欧拉角 -> 四元数
tf2::Quaternion euler_to_quaternion_tf(double roll, double pitch, double yaw)
{
    tf2::Quaternion q;
    q.setRPY(roll, pitch, yaw);  // 参数顺序：roll, pitch, yaw
    q.normalize();
    return q;
}

1.2.2 四元数 → 欧拉角（tf2）

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tuple>

// 四元数 -> 欧拉角
std::tuple<double, double, double> quaternion_to_euler_tf(const tf2::Quaternion& q_in)
{
    tf2::Quaternion q = q_in;
    q.normalize();

    tf2::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);  // 输出 roll, pitch, yaw
    return std::make_tuple(roll, pitch, yaw);
}

重要：setRPY 和 getRPY 是成对使用，互相兼容的；对于绝大多数 ROS 应用，你只需要遵循这对 API，而不必手动去记某种"ZYX/XYZ"的数学定义。

---

1.3 使用 GLM（OpenGL / 游戏常用数学库）

GLM（OpenGL Mathematics）广泛用于游戏和图形引擎里，API 风格类似 GLSL。

常用类型：

• glm::quat：四元数，分量顺序是 (w, x, y, z)
• glm::vec3：向量
• glm::mat3/mat4：矩阵

GLM 约定默认是右手坐标系，弧度制（建议开启 GLM_FORCE_RADIANS）。

1.3.1 欧拉角 → 四元数（GLM）

GLM 提供了 glm::quat(glm::vec3(eulerAngles)) 的构造重载，但其内部欧拉角顺序为 Z-Y-X（pitch-yaw-roll 的某种约定） ，并且有版本差异与坑。

更推荐"明确轴旋转"的写法：

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>

// 欧拉角->四元数 (roll, pitch, yaw) 弧度制，Z-Y-X 旋转顺序
glm::quat euler_to_quaternion_glm(float roll, float pitch, float yaw)
{
    glm::quat qx = glm::angleAxis(roll,  glm::vec3(1.0f, 0.0f, 0.0f));
    glm::quat qy = glm::angleAxis(pitch, glm::vec3(0.0f, 1.0f, 0.0f));
    glm::quat qz = glm::angleAxis(yaw,   glm::vec3(0.0f, 0.0f, 1.0f));

    // 先绕 X, 再绕 Y, 再绕 Z  => q = qz * qy * qx
    glm::quat q = qz * qy * qx;
    return glm::normalize(q);
}

1.3.2 四元数 → 欧拉角（GLM）

可以用 glm::eulerAngles(q)，返回的 vec3 中的含义要看文档（一般为 XYZ 顺序的欧拉角）。如果你保证创建时用的是 ZYX 组合方式，正反方向保持一致即可：

#include <glm/glm.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>

glm::vec3 quaternion_to_euler_glm(const glm::quat& q_in)
{
    glm::quat q = glm::normalize(q_in);
    // 返回的三分量通常可认为是 (pitchX, yawY, rollZ) 或 (roll, pitch, yaw) 视版本而定
    glm::vec3 euler = glm::eulerAngles(q);
    // 如果你要明确 roll, pitch, yaw 的含义，建议用约定统一它
    return euler;
}

GLM 对欧拉角的标注相对含糊，如想绝对严谨，推荐：只在 GLM 内使用四元数与矩阵，不在其中大量用欧拉角，或者写清自己的封装并测试正反变换。

---

二、Python 中使用常见库

2.1 使用 SciPy（推荐）

如果你可以使用 scipy，强烈推荐 scipy.spatial.transform.Rotation，设计良好、文档清晰。

安装（如需）：

pip install scipy

2.1.1 欧拉角 → 四元数（SciPy）

from scipy.spatial.transform import Rotation as R
import numpy as np

# 欧拉角 -> 四元数
def euler_to_quaternion_scipy(roll, pitch, yaw):
    """
    roll, pitch, yaw: 弧度
    约定：Z-Y-X (yaw, pitch, roll)，'zyx' 表示按照给定顺序应用旋转
    SciPy 的 from_euler('zyx', [yaw, pitch, roll]) 是最自然的写法
    """
    r = R.from_euler('zyx', [yaw, pitch, roll])   # 注意顺序是 [z, y, x] = [yaw, pitch, roll]
    # SciPy 的 as_quat() 返回顺序是 (x, y, z, w)
    x, y, z, w = r.as_quat()
    return np.array([w, x, y, z])  # 转为 (w, x, y, z)

如果你要批量处理（N×3 的欧拉角数组），from_euler 也支持批量。

2.1.2 四元数 → 欧拉角（SciPy）

from scipy.spatial.transform import Rotation as R
import numpy as np

def quaternion_to_euler_scipy(q):
    """
    q: 长度为 4 的数组/列表，顺序 (w, x, y, z)
    返回：roll, pitch, yaw（弧度）
    """
    q = np.asarray(q, dtype=float)
    w, x, y, z = q
    # SciPy Rotation 需要 (x, y, z, w)
    r = R.from_quat([x, y, z, w])
    # 'zyx' 对应 [yaw, pitch, roll]
    yaw, pitch, roll = r.as_euler('zyx')
    return roll, pitch, yaw

小结：

• SciPy 内部的 as_quat 始终是 (x, y, z, w)
• from_euler('zyx', [yaw, pitch, roll]) 明确地表示 Z-Y-X 顺序
• 你只需要在"自己的函数接口"层面，用好 (w, x, y, z) 和 (roll, pitch, yaw) 这套约定即可。

---

2.2 使用 ROS 的 tf_transformations（Python 版）

在 ROS1 中常见 tf.transformations 模块，许多 Python 节点会直接用它。

安装（如果不是在 ROS 环境，可以通过 pip 装一个兼容版本 tf-transformations）：

pip install tf-transformations

2.2.1 欧拉角 → 四元数（tf_transformations）

import tf_transformations as tf

def euler_to_quaternion_tf(roll, pitch, yaw):
    """
    tf.transformations.euler_from_quaternion / quaternion_from_euler
    使用的欧拉顺序默认为 (roll, pitch, yaw) 对应 XYZ
    """
    # 返回顺序为 (x, y, z, w)
    x, y, z, w = tf.quaternion_from_euler(roll, pitch, yaw)  
    return (w, x, y, z)

2.2.2 四元数 → 欧拉角（tf_transformations）

import tf_transformations as tf

def quaternion_to_euler_tf(q):
    """
    q: (w, x, y, z)
    返回 roll, pitch, yaw（弧度）
    """
    w, x, y, z = q
    # euler_from_quaternion 接受 (x, y, z, w)
    roll, pitch, yaw = tf.euler_from_quaternion([x, y, z, w])
    return roll, pitch, yaw

与 C++ 的 tf2 一样，quaternion_from_euler 与 euler_from_quaternion 是强绑定的一对，正反使用不会有旋转约定不一致的问题。

---

2.3 仅用 NumPy + 自实现（方便嵌入）

上一版已经提供了纯公式 + numpy 的实现，对多数不想依赖 SciPy 的场景就足够了：

• euler_to_quaternion(roll, pitch, yaw)
• quaternion_to_euler(q)

如果你只需要在一个无 ROS、无 SciPy 的环境中做数学转换，这一版即可；如果你已经有 SciPy / ROS，则优先用库函数，以减少出错风险。

---

三、如何在工程中选择/对齐这些库

1. 在 C++ 机器人系统里（ROS）

   • 建议：直接使用 tf2::Quaternion::setRPY 与 Matrix3x3::getRPY
   • 如果还用到了 Eigen（路径规划/控制算法），则在接口边界使用 Eigen ↔ tf2 的转换（把 (x,y,z,w) 显式地对号入座）。

2. 在 C++ 纯数学/控制项目里（无 ROS）

   • 强烈建议使用 Eigen 的 Quaterniond + AngleAxisd + eulerAngles
   • 不再自己写三角公式，除非你要做极致优化。

3. 在游戏/图形渲染里（C++/GLM）

   • 始终用 glm::quat + glm::mat4
   • 尽量避免大量欧拉角逻辑，只在输入输出/编辑器界面做"欧拉角接口"；内部用四元数和矩阵。

4. 在 Python 科学运算/姿态处理里

   • 有 SciPy：用 scipy.spatial.transform.Rotation
   • 在 ROS 中：用 tf.transformations 或 tf2 的接口，并在边界处转换为 SciPy/Eigen/自定义数学结构。

5. 统一几个关键点（无论你选哪个库都需要确认）：

   • 坐标系：右手/左手
   • 旋转顺序：XYZ / ZYX / ...
   • 角度单位：度 / 弧度
   • 四元数分量顺序：是 (x,y,z,w) 还是 (w,x,y,z)

一旦选定，就在项目里写一份简单的文档 + 封装函数，把所有库的外表现象统一成你自己定义的接口形式（例如：统一成 (w, x, y, z) + (roll, pitch, yaw, ZYX, 弧度)），避免在工程中混杂着不同语义的函数。