2. 坐标系与刚体变换：世界坐标系、机器人坐标系、齐次变换矩阵
做轨迹评估，说白了就是比较两条轨迹的差异。但有个前提——你得确保这两条轨迹是在同一个坐标系下说的。不然就像我在北京说「往东走500米」，你在上海也往东走500米，咱们说的根本不是一回事。
坐标系和刚体变换，就是解决这个「统一语言」的问题。我个人习惯，在开始任何SLAM项目之前，先把坐标系定义写死在代码注释里，省得后面扯皮。
2.1 世界坐标系与机器人坐标系
先搞清楚两个最常用的坐标系：
- 世界坐标系：固定不动的参考系。通常选在起点、房间角落或者某个已知地标上。它是绝对的，所有轨迹最终都要映射到这个坐标系下比较。
- 机器人坐标系：跟着机器人走的坐标系。原点在机器人中心，x轴朝前，y轴朝左或朝右（取决于你的定义）。它是相对的，传感器数据最初都是在这个坐标系下采集的。
你想想看，机器人每时每刻都在动，它的坐标系也在动。我们要做的，就是把每一时刻机器人坐标系下的观测，变换到世界坐标系下。这就是轨迹评估的第一步。
核心要点：轨迹评估时，真值轨迹和估计轨迹必须在同一个世界坐标系下。如果真值是全局坐标系，估计是局部坐标系，那直接算ATE（绝对轨迹误差）就是错的。
2.2 齐次变换矩阵
怎么把一个坐标系下的点，变到另一个坐标系下？用齐次变换矩阵。
一个3D刚体变换，包含旋转和平移。写成4x4矩阵就是：
T = [ R t ]
[ 0 1 ]其中R是3x3旋转矩阵，t是3x1平移向量。这个矩阵可以把一个齐次坐标点从坐标系A变换到坐标系B：
p_B = T_AB * p_A这里T_AB表示「从A到B的变换」。我刚开始做SLAM时，经常搞混下标顺序。后来养成一个习惯：下标从右往左读。T_AB就是把A坐标系下的点变换到B坐标系。
我的小技巧：在代码里，我习惯把变换矩阵命名为 T_world_robot 或者 T_w_r，一眼就能看出是从机器人坐标系到世界坐标系。别用 T1、T2 这种名字，三个月后你自己都看不懂。
2.3 旋转的表示方式
旋转矩阵有9个参数，但自由度只有3。所以实际中我们常用更紧凑的表示：
- 欧拉角：直观，但有万向锁问题。我在做无人机轨迹评估时吃过这个亏——俯仰角到90度时，偏航和滚转就分不清了。
- 四元数：无奇点，插值方便。SLAM领域的主流选择。
- 旋转向量：也叫轴角，李代数的基础。
我个人建议：内部计算用四元数，与人交互时转成欧拉角。但要注意，不同领域的欧拉角定义可能不同（ZYX还是XYZ？），一定要确认清楚。
2.4 轨迹评估中的坐标系对齐
回到轨迹评估。假设你有一个真值轨迹（世界坐标系下）和一个估计轨迹（也是世界坐标系下）。但实际中，估计轨迹往往存在一个固定的偏移——因为SLAM系统的初始位姿是任意的。
所以评估前，需要做一次对齐。常用的方法是：
- 用Umeyama算法或SVD，求解两个轨迹之间的最优刚体变换
- 把估计轨迹变换到真值轨迹的坐标系下
- 再计算误差指标
我曾经遇到一个坑：对齐时只用了平移，忽略了旋转。结果ATE看起来很小，但轨迹形状完全是歪的。嗯，这里要注意——对齐必须是完整的刚体变换（旋转+平移）。
避坑指南：我曾经在评估一个VIO系统时，发现ATE异常大。排查了半天，发现是估计轨迹和真值轨迹的坐标系定义不同——一个用右手系，一个用左手系。所以，第一步永远是检查坐标系定义是否一致。
2.5 知识结构图
下面这张图总结了本章的核心逻辑：
2.6 代码示例：齐次变换矩阵的构建
用Python和NumPy，构建一个从机器人坐标系到世界坐标系的变换矩阵：
import numpy as np
from scipy.spatial.transform import Rotation as R
def build_transform(translation, quaternion):
"""
构建齐次变换矩阵
:param translation: [x, y, z] 平移向量
:param quaternion: [w, x, y, z] 四元数
:return: 4x4 齐次变换矩阵
"""
rot_matrix = R.from_quat(quaternion).as_matrix()
T = np.eye(4)
T[:3, :3] = rot_matrix
T[:3, 3] = translation
return T
# 示例：机器人位于世界坐标系 (1, 2, 0) 处，朝向用四元数表示
trans = [1.0, 2.0, 0.0]
quat = [0.707, 0.0, 0.707, 0.0] # 绕z轴旋转90度
T_world_robot = build_transform(trans, quat)
print("齐次变换矩阵 T_world_robot:")
print(T_world_robot)提示：实际项目中，我习惯把变换矩阵的构建封装成一个工具函数，放在 utils/geometry.py 里。所有模块都调用同一个函数，避免各处重复实现导致的不一致。
2.7 小结
坐标系和刚体变换，是轨迹评估的地基。地基没打好，上面盖的房子再漂亮也是危房。记住三点：
- 明确世界坐标系和机器人坐标系的定义
- 用齐次变换矩阵统一坐标变换
- 评估前务必做坐标系对齐
下一章我们会深入ATE和RPE的具体计算方法，到时候你会发现，坐标系对齐做得好不好，直接决定了评估结果的可信度。