提供基于 C++（Eigen）和 Python（NumPy + SciPy）的姿态转换工具库实现。内容涵盖旋转矩阵、齐次变换矩阵、欧拉角（Z-Y-X 顺序）、四元数（wxyz 格式）之间的相互转换函数。文章明确了右手坐标系、单位及接口规范，提供了可直接复用的头文件与模块代码示例，并建议了项目结构与扩展方向，适用于机器人、控制及 SLAM 等领域的开发。
本文旨在基于 C++ 和 Python，使用常用库实现旋转矩阵、齐次变换矩阵、欧拉角、四元数之间的相互转换，并提供长期可复用的代码库。
下面所有 C++ / Python 代码都遵循同一套约定：
roll：绕 x 轴旋转pitch：绕 y 轴旋转yaw：绕 z 轴旋转R = Rz(yaw) * Ry(pitch) * Rx(roll)q = (w, x, y, z)(x, y, z, w)，我们在封装函数里统一成 (w, x, y, z) 接口。v_world = R * v_body（列向量右乘的习惯）pose_utils_eigen.h#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 <-> 欧拉角
// =============================
// 欧拉角 -> 旋转矩阵
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;
}
// 旋转矩阵 -> 欧拉角
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
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)
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++ 工程中作为公共工具。
假设你可以安装 SciPy。如果暂时没有 SciPy，也可以只用 NumPy + 手写公式。
pip install numpy scipy
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
在这个 C++ / Python 库中，实际上是围绕几条'主干'做封装：
欧拉角 ⇄ 旋转矩阵四元数 ⇄ 旋转矩阵(R, t) ⇄ T欧拉角 ⇄ 四元数：通过 欧拉角 -> R -> 四元数 / 四元数 -> R -> 欧拉角(欧拉角, t) ⇄ T：通过 欧拉角 -> R + rt ⇄ T(四元数, t) ⇄ T：通过 四元数 -> R + rt ⇄ T这样一方面接口清晰，另一方面将来如果你要替换内部实现（比如用手写三角公式、用别的库），只要保证每条主干的语义不变即可。
你可以按如下方式组织你的'长期自用库'：
目录示例：
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;
}
目录示例：
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)
以后你可以在这套基础上继续加：
euler_xyz_to_rot。*_deg 版本；或在函数里增加参数 degrees=False/True。from_tf_quat / to_tf_quat，做 (x,y,z,w) 与 (w,x,y,z) 的适配。T_ab、T_ba 一套函数，对逆变换进行封装。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online