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

这篇文章的目标是基于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=[R0​t1​]
   • 向量变换：
     pBhom=TBA pAhom p_B^{hom} = T_{BA} \, p_A^{hom}pBhom​=TBA​pAhom​
     即TBAT_{BA}TBA​表示“从 A 到 B”的变换（这一点你可以按自己项目统一命名）。

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

1.1 头文件示例：pose_utils_eigen.h

#pragmaonce#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]// -----------------------------// 基本类型别名// -----------------------------usingVec3=Eigen::Vector3d;usingMat3=Eigen::Matrix3d;usingMat4=Eigen::Matrix4d;usingQuat=Eigen::Quaterniond;// 内部存 (x, y, z, w)// =============================// R <-> 欧拉角// =============================// 欧拉角 -> 旋转矩阵// 输入: roll, pitch, yaw (rad)// 输出: R = Rz(yaw) * Ry(pitch) * Rx(roll)inlineMat3euler_to_rot(doubleroll,doublepitch,doubleyaw){Mat3 R;R=Eigen::AngleAxisd(yaw,Vec3::UnitZ())*Eigen::AngleAxisd(pitch,Vec3::UnitY())*Eigen::AngleAxisd(roll,Vec3::UnitX());returnR;}// 旋转矩阵 -> 欧拉角// 返回: (roll, pitch, yaw)inlineEigen::Vector3drot_to_euler(constMat3&R){// eulerAngles(2,1,0) -> [yaw, pitch, roll] for ZYXEigen::Vector3d euler=R.eulerAngles(2,1,0);doubleyaw=euler[0];doublepitch=euler[1];doubleroll=euler[2];returnEigen::Vector3d(roll,pitch,yaw);}// =============================// R <-> 四元数// =============================// 四元数(qw,qx,qy,qz) -> 旋转矩阵inlineMat3quat_to_rot(doublew,doublex,doubley,doublez){Quatq(x,y,z,w);// Eigen 构造: (x, y, z, w)q.normalize();returnq.toRotationMatrix();}// 旋转矩阵 -> 四元数(qw,qx,qy,qz)inlineEigen::Vector4drot_to_quat(constMat3&R){Quatq(R);q.normalize();returnEigen::Vector4d(q.w(),q.x(),q.y(),q.z());}// =============================// 欧拉角 <-> 四元数// =============================// 欧拉角 -> 四元数(qw,qx,qy,qz)inlineEigen::Vector4deuler_to_quat(doubleroll,doublepitch,doubleyaw){Mat3 R=euler_to_rot(roll,pitch,yaw);returnrot_to_quat(R);}// 四元数(qw,qx,qy,qz) -> 欧拉角(roll,pitch,yaw)inlineEigen::Vector3dquat_to_euler(doublew,doublex,doubley,doublez){Mat3 R=quat_to_rot(w,x,y,z);returnrot_to_euler(R);}// =============================// 齐次变换 T <-> (R, t)// =============================// (R, t) -> 齐次变换矩阵 T// T = [R t; 0 1]inlineMat4rt_to_T(constMat3&R,constVec3&t){Mat4 T=Mat4::Identity();T.topLeftCorner<3,3>()=R;T.topRightCorner<3,1>()=t;returnT;}// 齐次变换矩阵 T -> (R, t)inlinevoidT_to_rt(constMat4&T,Mat3&R,Vec3&t){R=T.topLeftCorner<3,3>();t=T.topRightCorner<3,1>();}// =============================// T <-> (欧拉角, t)// =============================// (欧拉角, t) -> TinlineMat4euler_t_to_T(doubleroll,doublepitch,doubleyaw,doubletx,doublety,doubletz){Mat3 R=euler_to_rot(roll,pitch,yaw);Vec3t(tx,ty,tz);returnrt_to_T(R,t);}// T -> (欧拉角, t)// 输出: roll,pitch,yaw 与 tx,ty,tzinlinevoidT_to_euler_t(constMat4&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) -> TinlineMat4quat_t_to_T(doublew,doublex,doubley,doublez,doubletx,doublety,doubletz){Mat3 R=quat_to_rot(w,x,y,z);Vec3t(tx,ty,tz);returnrt_to_T(R,t);}// T -> (四元数, t)inlinevoidT_to_quat_t(constMat4&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 依赖安装

pipinstallnumpy 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] """importnumpyasnpfromscipy.spatial.transformimportRotationasR_# 避免与变量名 R 冲突# =============================# R <-> 欧拉角# =============================defeuler_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])returnr.as_matrix()# (3,3)defrot_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-Xreturnnp.array([roll,pitch,yaw])# =============================# R <-> 四元数# =============================defquat_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]returnr.as_matrix()defrot_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]returnnp.array([w,x,y,z])# =============================# 欧拉角 <-> 四元数# =============================defeuler_to_quat(roll:float,pitch:float,yaw:float)->np.ndarray:""" 欧拉角 -> 四元数 返回: [w, x, y, z] """R=euler_to_rot(roll,pitch,yaw)returnrot_to_quat(R)defquat_to_euler(q:np.ndarray)->np.ndarray:""" 四元数 -> 欧拉角 q: [w, x, y, z] 返回: [roll, pitch, yaw] """R=quat_to_rot(q)returnrot_to_euler(R)# =============================# 齐次变换 T <-> (R, t)# =============================defrt_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]=treturnTdefT_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]returnR,t# =============================# T <-> (欧拉角, t)# =============================defeuler_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)returnrt_to_T(R,t)defT_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=treturnroll,pitch,yaw,tx,ty,tz# =============================# T <-> (四元数, t)# =============================defquat_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)returnrt_to_T(R,t)defT_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)returnq,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"intmain(){doubleroll=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);return0;}

Python

目录示例：

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

使用：

frompose_utils.pose_utilsimporteuler_to_quat,quat_to_euler,euler_t_to_T roll,pitch,yaw=0.1,0.2,0.3q=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一套函数，对逆变换进行封装。