3. 绝对轨迹误差（ATE）：从定义到实战

ATE，全称 Absolute Trajectory Error。说白了，就是衡量你估计的轨迹和真实轨迹之间，到底差了多少。

我刚开始做SLAM那会儿，总觉得ATE是个很玄乎的东西。后来踩过几次坑才明白，它其实就是把两段轨迹对齐后，逐帧算位置差。嗯，就这么简单。

3.1 ATE到底在算什么？

想象一下，你手里有两段轨迹：

真实轨迹（Ground Truth）：通常来自高精度设备，比如动捕系统、激光跟踪仪
估计轨迹（Estimated）：你的SLAM算法算出来的

ATE要做的，就是先把这两段轨迹对齐（因为坐标系可能不同），然后逐帧计算位置误差。注意，这里只算位置，不算姿态。

核心公式：

对于第 i 帧，ATE 定义为：

ATE_i = || P_est,i - (s * R * P_gt,i + t) ||

其中：

P_est,i：估计轨迹的第 i 个位置点
P_gt,i：真实轨迹的第 i 个位置点
s：尺度因子（单目SLAM需要）
R, t：旋转和平移，用于对齐坐标系

3.2 对齐这一步，很多人会忽略

我见过不少新手，直接把两段轨迹拿来算差。结果误差大得离谱，还以为是算法不行。

其实，真实轨迹和估计轨迹通常不在同一个坐标系下。比如真实轨迹是动捕系统给的，原点在房间角落；估计轨迹是相机坐标系，原点在起始位置。你不对齐，算出来的ATE毫无意义。

对齐方式有两种：

Sim(3)对齐：适用于单目SLAM，需要估计尺度、旋转、平移
SE(3)对齐：适用于双目或RGB-D，只需要旋转和平移

我个人习惯用 evo 工具包里的对齐函数，它默认用 Umeyama 算法做 Sim(3) 对齐。你想想看，如果尺度不对，ATE 能准吗？

3.3 RMSE vs 均值：哪个更靠谱？

好，对齐完了，我们得到了每一帧的 ATE_i。接下来要算一个总体的指标。常用的有两个：

指标	公式	特点
RMSE	sqrt( (1/N) * Σ ATE_i² )	对大误差敏感，能反映最差情况
均值	(1/N) * Σ ATE_i	反映整体水平，但会掩盖局部大误差

避坑指南：

我曾经只用均值来评估，结果算法在大部分帧表现不错，但偶尔有几帧漂移很大。均值看起来还行，RMSE却高得吓人。后来我养成了习惯：RMSE和均值一起看。如果两者差距很大，说明存在明显的异常帧。

3.4 代码实战：手算ATE

光说不练假把式。我们来看看怎么用Python算ATE。这里我用的是 numpy 和 scipy：

import numpy as np
from scipy.spatial.transform import Rotation as R

def compute_ate(est_poses, gt_poses):
    """
    est_poses: (N, 7) [x, y, z, qx, qy, qz, qw]
    gt_poses:  (N, 7) [x, y, z, qx, qy, qz, qw]
    """
    # 1. 提取位置
    est_pos = est_poses[:, :3]
    gt_pos = gt_poses[:, :3]
    
    # 2. Sim(3)对齐（这里用Umeyama算法）
    # 实际项目中建议用 evo 或 Sophus 库
    # 这里简化处理，假设已经对齐
    
    # 3. 逐帧计算ATE
    ate_list = []
    for i in range(len(est_pos)):
        error = np.linalg.norm(est_pos[i] - gt_pos[i])
        ate_list.append(error)
    
    ate_array = np.array(ate_list)
    
    # 4. 计算RMSE和均值
    rmse = np.sqrt(np.mean(ate_array ** 2))
    mean = np.mean(ate_array)
    
    return rmse, mean, ate_array

我的小技巧：

在实际项目中，我一般会同时输出ATE的最大值和标准差。最大值告诉你最差的情况，标准差告诉你误差的波动程度。这两个指标配合RMSE，基本能全面评估轨迹质量。

3.5 ATE的局限性

ATE虽然好用，但也不是万能的。我遇到过这样的情况：

ATE很低，但轨迹局部有抖动（ATE只关心位置，不关心平滑度）
ATE很低，但姿态误差很大（ATE只算位置，不算旋转）

所以，我一般会配合相对轨迹误差（RPE）一起使用。RPE能反映局部漂移和姿态误差，和ATE互补。这个我们下一节会详细讲。

3.6 知识体系总览

下面这张图，是我自己总结的ATE核心逻辑。你看一遍，基本就全明白了：

嗯，这张图把ATE的整个流程串起来了。从输入到对齐，再到逐帧计算，最后输出指标。你照着这个思路去实现，基本不会出错。

可可精品资料库