旋转矩阵，齐次变换矩阵，欧拉角，四元数等相互转换的常用代码C++ Python

Tipriest_2025-12-16 13:06

这篇文章的目标是基于C++和Python，使用一些常用的库，将旋转矩阵，齐次变换矩阵，欧拉角，四元数等之间进行两两的相互转换，并能够作为长期可复用的库来进行使用

目标：

  • 统一约定（坐标系、欧拉顺序、四元数形式等）
  • C++：基于 Eigen（机器人/控制/SLAM 领域最常用）
  • Python：基于 NumPy + SciPy Rotation（可选 ROS 的 tf，但先给通用版）
  • 提供一组简洁、可直接粘贴的函数，带短注释，方便做成你的"姿态库"。

你可以直接把"核心约定 + 代码段"收进自己的项目中。

0. 全局统一约定（极其重要）

下面所有 C++ / Python 代码都遵循同一套约定：

  1. 右手坐标系：

    • x 轴：前
    • y 轴：左
    • z 轴：上
      （典型移动机器人/航空/SLAM 约定）

  2. 欧拉角定义：

    • roll：绕 x 轴旋转
    • pitch：绕 y 轴旋转
    • yaw：绕 z 轴旋转
    • 采用 Z-Y-X 顺序（yaw, pitch, roll） ，即
      先绕 x（roll），再绕 y（pitch），再绕 z（yaw）的本体旋转 等价于
      旋转矩阵：R = Rz(yaw) * Ry(pitch) * Rx(roll)

  3. 四元数形式：

    • 使用数学常用表示：q = (w, x, y, z)
    • 内部库如 Eigen / SciPy 可能用 (x, y, z, w)，我们在封装函数里统一成 (w, x, y, z) 接口。

  4. 旋转矩阵 R：

    • 3×3 正交矩阵，列向量为坐标轴在世界坐标中的方向
    • 满足 v_world = R * v_body（列向量右乘的习惯）

  5. 齐次变换矩阵 T (4×4)：

    • T 表示从 A 坐标系到 B 坐标系的刚体变换
    • 形式：
      T=[Rt01] T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} T=[R0t1]
    • 向量变换：
      pBhom=TBA pAhom p_B^{hom} = T_{BA} \, p_A^{hom} pBhom=TBApAhom
      即 TBAT_{BA}TBA 表示"从 A 到 B"的变换（这一点你可以按自己项目统一命名）。

1. C++ 版（基于 Eigen）的统一接口

1.1 头文件示例：pose_utils_eigen.h

cpp 复制代码 
#pragma once

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

// 约定：
// - 欧拉角顺序 Z-Y-X：R = Rz(yaw) * Ry(pitch) * Rx(roll)
// - 欧拉角单位：弧度
// - 四元数接口形式： (w, x, y, z)
// - 旋转矩阵 R: 3x3, v_world = R * v_body
// - 齐次变换 T: 4x4 = [R t; 0 1]

// -----------------------------
// 基本类型别名
// -----------------------------

using Vec3   = Eigen::Vector3d;
using Mat3   = Eigen::Matrix3d;
using Mat4   = Eigen::Matrix4d;
using Quat   = Eigen::Quaterniond; // 内部存 (x, y, z, w)

// =============================
// R <-> 欧拉角
// =============================

// 欧拉角 -> 旋转矩阵
// 输入: roll, pitch, yaw (rad)
// 输出: R = Rz(yaw) * Ry(pitch) * Rx(roll)
inline Mat3 euler_to_rot(double roll, double pitch, double yaw)
{
    Mat3 R;
    R = Eigen::AngleAxisd(yaw,   Vec3::UnitZ()) *
        Eigen::AngleAxisd(pitch, Vec3::UnitY()) *
        Eigen::AngleAxisd(roll,  Vec3::UnitX());
    return R;
}

// 旋转矩阵 -> 欧拉角
// 返回: (roll, pitch, yaw)
inline Eigen::Vector3d rot_to_euler(const Mat3& R)
{
    // eulerAngles(2,1,0) -> [yaw, pitch, roll] for ZYX
    Eigen::Vector3d euler = R.eulerAngles(2, 1, 0);
    double yaw   = euler[0];
    double pitch = euler[1];
    double roll  = euler[2];
    return Eigen::Vector3d(roll, pitch, yaw);
}

// =============================
// R <-> 四元数
// =============================

// 四元数(qw,qx,qy,qz) -> 旋转矩阵
inline Mat3 quat_to_rot(double w, double x, double y, double z)
{
    Quat q(x, y, z, w);  // Eigen 构造: (x, y, z, w)
    q.normalize();
    return q.toRotationMatrix();
}

// 旋转矩阵 -> 四元数(qw,qx,qy,qz)
inline Eigen::Vector4d rot_to_quat(const Mat3& R)
{
    Quat q(R);
    q.normalize();
    return Eigen::Vector4d(q.w(), q.x(), q.y(), q.z());
}

// =============================
// 欧拉角 <-> 四元数
// =============================

// 欧拉角 -> 四元数(qw,qx,qy,qz)
inline Eigen::Vector4d euler_to_quat(double roll, double pitch, double yaw)
{
    Mat3 R = euler_to_rot(roll, pitch, yaw);
    return rot_to_quat(R);
}

// 四元数(qw,qx,qy,qz) -> 欧拉角(roll,pitch,yaw)
inline Eigen::Vector3d quat_to_euler(double w, double x, double y, double z)
{
    Mat3 R = quat_to_rot(w, x, y, z);
    return rot_to_euler(R);
}

// =============================
// 齐次变换 T <-> (R, t)
// =============================

// (R, t) -> 齐次变换矩阵 T
// T = [R t; 0 1]
inline Mat4 rt_to_T(const Mat3& R, const Vec3& t)
{
    Mat4 T = Mat4::Identity();
    T.topLeftCorner<3,3>() = R;
    T.topRightCorner<3,1>() = t;
    return T;
}

// 齐次变换矩阵 T -> (R, t)
inline void T_to_rt(const Mat4& T, Mat3& R, Vec3& t)
{
    R = T.topLeftCorner<3,3>();
    t = T.topRightCorner<3,1>();
}

// =============================
// T <-> (欧拉角, t)
// =============================

// (欧拉角, t) -> T
inline Mat4 euler_t_to_T(double roll, double pitch, double yaw,
                         double tx, double ty, double tz)
{
    Mat3 R = euler_to_rot(roll, pitch, yaw);
    Vec3 t(tx, ty, tz);
    return rt_to_T(R, t);
}

// T -> (欧拉角, t)
// 输出: roll,pitch,yaw 与 tx,ty,tz
inline void T_to_euler_t(const Mat4& T,
                         double& roll, double& pitch, double& yaw,
                         double& tx, double& ty, double& tz)
{
    Mat3 R;
    Vec3 t;
    T_to_rt(T, R, t);
    Eigen::Vector3d rpy = rot_to_euler(R);
    roll  = rpy[0];
    pitch = rpy[1];
    yaw   = rpy[2];
    tx = t[0];
    ty = t[1];
    tz = t[2];
}

// =============================
// T <-> (四元数, t)
// =============================

// (四元数, t) -> T
inline Mat4 quat_t_to_T(double w, double x, double y, double z,
                        double tx, double ty, double tz)
{
    Mat3 R = quat_to_rot(w, x, y, z);
    Vec3 t(tx, ty, tz);
    return rt_to_T(R, t);
}

// T -> (四元数, t)
inline void T_to_quat_t(const Mat4& T,
                        double& w, double& x, double& y, double& z,
                        double& tx, double& ty, double& tz)
{
    Mat3 R;
    Vec3 t;
    T_to_rt(T, R, t);
    Eigen::Vector4d q = rot_to_quat(R);
    w = q[0];
    x = q[1];
    y = q[2];
    z = q[3];
    tx = t[0];
    ty = t[1];
    tz = t[2];
}

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

2. Python 版（NumPy + SciPy Rotation）

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

2.1 依赖安装

bash 复制代码 
pip install numpy scipy

2.2 模块示例：pose_utils.py

python 复制代码 
"""
统一的姿态/位姿转换工具
约定:
- 右手系
- 欧拉角(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++

目录示例：

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

在项目中使用：

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

目录示例：

text 复制代码 
pose_utils/
  __init__.py
  pose_utils.py
tests/
  test_pose_utils.py

使用：

python 复制代码 
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 一套函数，对逆变换进行封装。
相关推荐
周杰伦_Jay5 小时前
【Go/Python/Java】基础语法+核心特性对比
java·python·golang
小鹿学程序5 小时前
jdk配置完之后java -version还是默认的jdk版本如何更改
java·开发语言·python
hz_zhangrl5 小时前
CCF-GESP 等级考试 2025年9月认证C++六级真题解析
c++·算法·青少年编程·程序设计·gesp·2025年9月gesp·gesp c++六级
Pyeako5 小时前
Python数据可视化--matplotlib库
python·matplotlib·数据可视化·画图·pylab
m0_704887895 小时前
Day 35
python·深度学习·机器学习
华研前沿标杆游学5 小时前
参观深圳比亚迪总部，探索科技,感受中国“智”造魅力
python
爱打代码的小林6 小时前
python基础（逻辑回归例题）
开发语言·python·逻辑回归
qq_214782616 小时前
pandas“将”迎来v3.0.0大版本更新！
python·pandas
兵哥工控6 小时前
MFC用高精度计时器实现五段时序控制器
c++·mfc·高精度计时器·时序控制器
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03Linux下V2Ray安装配置指南04【AutoGLM部署】本地私有化部署AI手机Agent05Open-AutoGLM Windows 安装部署教程06Cursor 又偷偷更新，这个功能太实用：Visual Editor for Cursor Browser07【超详细教程】手把手教你从微软官网免费下载Windows 10官方原版ISO镜像（2025最新版）08在VSCode配置Java开发环境的保姆级教程（适配各类AI编程IDE）09安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）10BongoCat - 跨平台键盘猫动画工具