旋转矩阵、齐次变换矩阵、欧拉角与四元数相互转换 C++ Python 实现

你可以把这整个头文件直接收录到自己的 C++ 工程中作为公共工具。

2. Python 版（NumPy + SciPy Rotation）

假设你可以安装 SciPy。如果暂时没有 SciPy，也可以只用 NumPy + 手写公式。

2.1 依赖安装

pip install numpy scipy

2.2 模块示例：pose_utils.py

""" 统一的姿态/位姿转换工具
约定:
- 右手系
- 欧拉角(roll, pitch, yaw), 旋转顺序 Z-Y-X: R = Rz(yaw) * Ry(pitch) * Rx(roll)
- 四元数接口形式 (w, x, y, z)
- 旋转矩阵 R: shape (3,3), v_world = R @ v_body
- 齐次变换 T: shape (4,4) = [R t; 0 1]
"""
import numpy as np
from scipy.spatial.transform import Rotation as R_

# 避免与变量名 R 冲突
# =============================
# R <-> 欧拉角
# =============================
def euler_to_rot(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """ 欧拉角 -> 旋转矩阵
    输入单位：弧度
    顺序：Z-Y-X (yaw, pitch, roll)
    """
    # SciPy: from_euler('zyx', [z, y, x]) = [yaw, pitch, roll]
    r = R_.from_euler('zyx', [yaw, pitch, roll])
    return r.as_matrix()  # (3,3)

def rot_to_euler(R: np.ndarray) -> np.ndarray:
    """ 旋转矩阵 -> 欧拉角
    返回：[roll, pitch, yaw] (弧度)
    """
    R = np.asarray(R, dtype=float).reshape(3, 3)
    r = R_.from_matrix(R)
    yaw, pitch, roll = r.as_euler('zyx')  # 对应 Z-Y-X
    return np.array([roll, pitch, yaw])

# =============================
# R <-> 四元数
# =============================
def quat_to_rot(q: np.ndarray) -> np.ndarray:
    """ 四元数 -> 旋转矩阵
    q: [w, x, y, z]
    返回：R (3,3)
    """
    q = np.asarray(q, dtype=float).flatten()
    w, x, y, z = q
    r = R_.from_quat([x, y, z, w])  # SciPy 使用 [x,y,z,w]
    return r.as_matrix()

def rot_to_quat(R: np.ndarray) -> np.ndarray:
    """ 旋转矩阵 -> 四元数
    返回：[w, x, y, z]
    """
    R = np.asarray(R, dtype=float).reshape(3, 3)
    r = R_.from_matrix(R)
    x, y, z, w = r.as_quat()  # SciPy 返回 [x,y,z,w]
    return np.array([w, x, y, z])

# =============================
# 欧拉角 <-> 四元数
# =============================
def euler_to_quat(roll: float, pitch: float, yaw: float) -> np.ndarray:
    """ 欧拉角 -> 四元数
    返回：[w, x, y, z]
    """
    R = euler_to_rot(roll, pitch, yaw)
    return rot_to_quat(R)

def quat_to_euler(q: np.ndarray) -> np.ndarray:
    """ 四元数 -> 欧拉角
    q: [w, x, y, z]
    返回：[roll, pitch, yaw]
    """
    R = quat_to_rot(q)
    return rot_to_euler(R)

# =============================
# 齐次变换 T <-> (R, t)
# =============================
def rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
    """ (R, t) -> 齐次变换矩阵 T
    R: (3,3) t: (3,)
    返回：T (4,4) = [R t; 0 1]
    """
    R = np.asarray(R, dtype=float).reshape(3, 3)
    t = np.asarray(t, dtype=float).reshape(3,)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

def T_to_rt(T: np.ndarray):
    """ 齐次变换矩阵 T -> (R, t)
    返回：R(3,3), t(3,)
    """
    T = np.asarray(T, dtype=float).reshape(4, 4)
    R = T[:3, :3]
    t = T[:3, 3]
    return R, t

# =============================
# T <-> (欧拉角, t)
# =============================
def euler_t_to_T(roll: float, pitch: float, yaw: float, tx: float, ty: float, tz: float) -> np.ndarray:
    """ (欧拉角, t) -> T
    """
    R = euler_to_rot(roll, pitch, yaw)
    t = np.array([tx, ty, tz], dtype=float)
    return rt_to_T(R, t)

def T_to_euler_t(T: np.ndarray):
    """ T -> (欧拉角, t)
    返回：roll, pitch, yaw, tx, ty, tz
    """
    R, t = T_to_rt(T)
    roll, pitch, yaw = rot_to_euler(R)
    tx, ty, tz = t
    return roll, pitch, yaw, tx, ty, tz

# =============================
# T <-> (四元数, t)
# =============================
def quat_t_to_T(q: np.ndarray, t: np.ndarray) -> np.ndarray:
    """ (四元数, t) -> T
    q: [w, x, y, z] t: (3,)
    """
    R = quat_to_rot(q)
    return rt_to_T(R, t)

def T_to_quat_t(T: np.ndarray):
    """ T -> (四元数, t)
    返回：q: [w, x, y, z], t: (3,)
    """
    R, t = T_to_rt(T)
    q = rot_to_quat(R)
    return q, t

3. 转换关系一览（思维导图式）

在这个 C++ / Python 库中，实际上是围绕几条'主干'做封装：

• 欧拉角 ⇄ 旋转矩阵
• 四元数 ⇄ 旋转矩阵
• (R, t) ⇄ T
• 其他组合，例如：
  • 欧拉角 ⇄ 四元数：通过 欧拉角 -> R -> 四元数 / 四元数 -> R -> 欧拉角
  • (欧拉角, t) ⇄ T：通过 欧拉角 -> R + rt ⇄ T
  • (四元数, t) ⇄ T：通过 四元数 -> R + rt ⇄ T

这样一方面接口清晰，另一方面将来如果你要替换内部实现（比如用手写三角公式、用别的库），只要保证每条主干的语义不变即可。

4. 建议的项目结构与使用方式

你可以按如下方式组织你的'长期自用库'：

C++

目录示例：

include/
  pose_utils_eigen.h
src/
  # 不一定需要单独 cpp，全部 inline 在头文件也可以
tests/
  test_pose_utils.cpp

在项目中使用：

#include "pose_utils_eigen.h"

int main() {
    double roll = 0.1, pitch = 0.2, yaw = 0.3;
    Eigen::Matrix3d R = euler_to_rot(roll, pitch, yaw);
    Eigen::Vector4d q = rot_to_quat(R);
    Eigen::Matrix4d T = euler_t_to_T(roll, pitch, yaw, 1.0, 2.0, 3.0);
    return 0;
}

Python

目录示例：

pose_utils/
  __init__.py
  pose_utils.py
tests/
  test_pose_utils.py

使用：

from pose_utils.pose_utils import euler_to_quat, quat_to_euler, euler_t_to_T

roll, pitch, yaw = 0.1, 0.2, 0.3
q = euler_to_quat(roll, pitch, yaw)
T = euler_t_to_T(roll, pitch, yaw, 1.0, 2.0, 3.0)

5. 后续可以扩展的方向（预留）

以后你可以在这套基础上继续加：

• 不同欧拉顺序：例如 XYZ、ZXY 等，在函数名中显式写明，比如 euler_xyz_to_rot。
• 度/弧度转换：提供 *_deg 版本；或在函数里增加参数 degrees=False/True。
• 与 ROS tf / tf2 的互操作：在 C++ / Python 分别写 from_tf_quat / to_tf_quat，做 (x,y,z,w) 与 (w,x,y,z) 的适配。
• 转移矩阵方向区分：例如 T_ab、T_ba 一套函数，对逆变换进行封装。