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

各位同学，今天我们来聊聊 SLAM 评估里最核心的指标之一——绝对轨迹误差（ATE）。

说实话，我刚开始做 SLAM 的时候，对 ATE 的理解也就停留在「算个 RMSE」的层面。直到有一次，我拿着一个看起来 RMSE 很低的轨迹去汇报，结果被导师一眼看穿问题——「你这轨迹整体漂移不大，但局部抖动很厉害啊」。嗯，从那天起我才真正开始认真琢磨 ATE 到底在算什么。

3.1 ATE 到底在衡量什么？

绝对轨迹误差，说白了就是：你估计的轨迹，和真实轨迹之间，差了多少。

你想想看，我们做 SLAM 最终要的是什么？是让机器人知道自己「在哪儿」。ATE 就是直接回答这个问题的——它衡量的是每个时间点上，估计位姿和真值位姿之间的偏差。

这里有个关键点：ATE 是「绝对」的。什么意思？它关心的是全局坐标系下的位置误差，而不是相对运动误差。比如你走了一个 10 米的走廊，ATE 会告诉你每个时刻你估计的位置和真实位置差了多少厘米。

核心定义：给定估计轨迹 P = {p₁, p₂, ..., pₙ} 和真实轨迹 Q = {q₁, q₂, ..., qₙ}，ATE 计算的是经过最优刚体变换对齐后，每个对应位姿之间的平移误差。

3.2 计算方法：一步步拆解

计算 ATE 其实就三步，我习惯这么记：对齐 → 求差 → 统计。

第一步：轨迹对齐

为什么需要对齐？因为估计轨迹和真实轨迹通常不在同一个坐标系下。你想想，你的 SLAM 系统可能以第一帧为原点，而真值可能是 GPS 坐标系。所以我们需要找一个刚体变换 T，让两个轨迹尽可能重合。

这个变换通常用 Umeyama 算法求解，其实就是最小二乘意义上的最优对齐。我在项目中遇到过一个问题：如果轨迹太短或者运动太单一，这个对齐会很不稳定。所以建议至少要有几米的运动距离再做 ATE 评估。

第二步：计算每个时间点的误差

对齐之后，对于每个时间点 i，我们计算：

e_i = ||trans(p_i) - trans(T * q_i)||

这里 trans() 是取平移向量。注意，我们通常只关心平移误差，旋转误差一般用 RPE 来评估。

第三步：统计汇总

得到所有时间点的误差后，我们通常用 RMSE 或均值来汇总。下面这张图展示了整个流程：

ATE 计算流程 估计轨迹 P 真实轨迹 Q 对齐 逐点求差 e_i e_i = ||trans(p_i) - trans(T*q_i)|| 统计汇总 RMSE = sqrt(mean(e_i²)) Mean = mean(e_i) ATE 评估结果

3.3 RMSE 与均值：到底该用哪个？

这是很多初学者会困惑的地方。我直接说结论：RMSE 比均值更常用，也更有意义。

为什么？因为 RMSE 对大的误差更敏感。你想想看，如果你的轨迹大部分时间误差只有 5cm，但中间有一次跳到了 20cm，均值可能只涨到 6cm，而 RMSE 会明显变大。这其实是我们想要的——大的漂移应该被惩罚。

指标 计算公式 特点 我推荐的使用场景
RMSE sqrt(mean(e_i²)) 对大误差敏感，能反映最差情况 论文、竞赛、正式评估
Mean mean(e_i) 反映平均表现，但会掩盖局部问题 快速调试、初步筛选
Median median(e_i) 对异常值鲁棒 数据有离群点时使用
Max max(e_i) 反映最坏情况 安全关键应用

我的个人习惯：在论文里同时报告 RMSE 和 Mean。RMSE 作为主要指标，Mean 作为辅助参考。如果两者差距很大，说明轨迹存在局部大误差，需要进一步分析。

3.4 代码实现：手写一个 ATE 计算

光说不练假把式。下面我给出一段 Python 代码，实现 ATE 的核心计算。这段代码我用了很多年，基本没改过。

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

def compute_ate(est_poses, gt_poses):
    """
    计算绝对轨迹误差
    est_poses: 估计位姿列表，每个元素是 4x4 变换矩阵
    gt_poses: 真实位姿列表，格式同上
    """
    n = len(est_poses)
    errors = []
    
    # 提取平移向量
    est_trans = np.array([p[:3, 3] for p in est_poses])
    gt_trans = np.array([p[:3, 3] for p in gt_poses])
    
    # 轨迹对齐（Umeyama 算法）
    # 这里简化处理，实际建议用 evo 库
    centroid_est = np.mean(est_trans, axis=0)
    centroid_gt = np.mean(gt_trans, axis=0)
    est_centered = est_trans - centroid_est
    gt_centered = gt_trans - centroid_gt
    
    # 计算最优旋转
    H = est_centered.T @ gt_centered
    U, _, Vt = np.linalg.svd(H)
    R_opt = Vt.T @ U.T
    if np.linalg.det(R_opt) < 0:
        Vt[-1, :] *= -1
        R_opt = Vt.T @ U.T
    
    # 计算最优平移
    t_opt = centroid_gt - R_opt @ centroid_est
    
    # 对齐后计算每个点的误差
    for i in range(n):
        est_aligned = R_opt @ est_trans[i] + t_opt
        error = np.linalg.norm(est_aligned - gt_trans[i])
        errors.append(error)
    
    errors = np.array(errors)
    rmse = np.sqrt(np.mean(errors ** 2))
    mean = np.mean(errors)
    median = np.median(errors)
    std = np.std(errors)
    
    return {
        'rmse': rmse,
        'mean': mean,
        'median': median,
        'std': std,
        'errors': errors
    }

我曾经踩过的坑：直接用上面的简化对齐代码处理大角度旋转的轨迹，结果 RMSE 算出来偏大很多。后来才发现，Umeyama 算法在处理纯旋转时会有数值问题。建议直接用 evo 库的 evo_ape 命令，它处理了各种边界情况。

3.5 实际使用中的注意事项

最后，我总结几个实际项目中容易忽略的点：

  • 时间戳对齐：估计轨迹和真实轨迹的时间戳往往不一致，需要插值。我习惯用线性插值，但如果运动很快，建议用样条插值。
  • 部分遮挡：如果真值数据有缺失，不要直接跳过。我曾经犯过这个错，结果 RMSE 看起来很好，但实际上是因为跳过了最难的部分。
  • 尺度问题：单目 SLAM 有尺度模糊性。计算 ATE 前需要先估计尺度因子，或者用 Sim(3) 对齐。这个我们后面章节会详细讲。
  • 不要只看数字：RMSE 0.1m 不一定比 0.15m 好。如果 0.1m 的轨迹在某个区域有 0.5m 的跳变，而 0.15m 的轨迹很平滑，那后者在实际应用中可能更可靠。

嗯，关于 ATE 的核心内容就这些。记住，评估指标只是工具，真正重要的是理解你的 SLAM 系统在什么场景下表现好、什么场景下会出问题。下一节我们会聊相对轨迹误差（RPE），它和 ATE 是互补的关系。

一句话总结：ATE 回答「你估计的位置有多准」，RMSE 比均值更能反映最差情况，但永远不要只看一个数字。

可可精品资料库

← 上一章 📖 返回目录 下一章 →