📚 课程目录第1章位姿估计概述

1. 位姿估计概述

各位同学好，我是这门课的主讲。咱们今天先聊点基础的——位姿估计到底是什么。

说白了，位姿估计就是回答两个问题：“我在哪？” 和 “我朝哪看？”。“位”是位置（x, y, z），“姿”是姿态（roll, pitch, yaw 或者四元数）。合在一起，就是刚体在三维空间里的 6 个自由度。

我记得刚入行那会儿，带我的老工程师跟我说：“你搞懂了位姿，就搞懂了一半的机器人视觉。”当时我不信，后来踩了无数坑才明白——这话一点不夸张。

想象一下，你手里拿着一个手机，打开 AR 测量 App，对着桌子扫一圈，然后屏幕上出现一条虚拟的线，告诉你桌子的长度。这个过程中，手机每时每刻都在做位姿估计——它要知道自己相对于桌子的位置和朝向，才能把虚拟物体“锚定”在真实空间里。

从数学上讲，位姿估计就是求解一个刚体变换矩阵 T ∈ SE(3)：

T = [ R  t ]
    [ 0  1 ]

其中 R 是旋转矩阵（3×3），t 是平移向量（3×1）。这个矩阵把世界坐标系下的点，映射到相机坐标系下。

核心要点：位姿估计不是单一算法，而是一套“传感器数据 → 特征匹配 → 几何约束 → 优化求解”的流水线。任何一个环节出问题，精度都会崩。

位姿估计的应用比你想象的更普遍。我简单列几个方向，你看看是不是都接触过：

领域	典型应用	精度要求	我踩过的坑
AR/VR	手机 AR 测量、Hololens 空间锚点	厘米级	曾经在弱纹理白墙上做 AR 标注，位姿直接漂到隔壁房间
机器人	机械臂抓取、AGV 导航	毫米级	有一次机械臂抓取螺丝，位姿误差 2mm，夹爪直接把零件弹飞了
自动驾驶	车辆定位、高精地图构建	厘米级（局部）	隧道里 GPS 失效，纯靠视觉里程计撑了 3 公里，回来一看轨迹偏了半米
无人机	视觉悬停、自主返航	分米级	大疆的 VIO 在强光下出现过一次位姿跳变，差点炸机

你想想看，这些场景对精度的要求完全不是一个量级。AR 差个几厘米，用户顶多觉得“有点歪”；但自动驾驶差 10 厘米，可能就是一场事故。所以，精度评价就成了位姿估计里绕不开的一环。

很多初学者喜欢问：“我这个算法精度怎么样？”我一般会反问：“你用什么指标？”

位姿估计的精度评价，主流就两个指标：RPE（相对位姿误差） 和 ATE（轨迹误差）。它们俩看问题的角度不一样。

RPE 衡量的是相邻两帧之间的位姿变化误差。说白了，它关心的是你的算法在短时间内的“平滑度”和“一致性”。

公式长这样：

E_RPE = (T_est_inv * T_gt) 的平移部分和旋转部分

其中 T_est 是估计的帧间变换，T_gt 是真实值。通常我们会取一个固定时间间隔 Δ，然后计算所有间隔的 RMSE。

我的习惯：在做 VIO 调试时，我第一个看的指标就是 RPE。如果 RPE 突然变大，十有八九是特征匹配出错了，或者 IMU 预积分有累积误差。RPE 能帮你快速定位“哪一段轨迹出了问题”。

ATE 衡量的是整条轨迹与真实轨迹之间的偏差。它更关心你的算法在长时间运行后，位置漂移了多少。

计算方式：先把估计轨迹和真实轨迹对齐（通常用 Umeyama 算法做相似变换对齐），然后逐帧计算位置误差：

ATE_RMSE = sqrt( 1/N * Σ ||p_est_i - p_gt_i||² )

其中 p_est_i 和 p_gt_i 是对齐后的位置点。

注意：ATE 对轨迹的“整体形状”很敏感。如果你的算法在局部表现很好（RPE 很低），但整体在缓慢漂移（ATE 很高），那说明你的算法缺少全局约束——比如没有闭环检测。我曾经在一个长走廊数据集上吃过这个亏，RPE 漂亮得很，ATE 却惨不忍睹。

为了让你对整章内容有个整体印象，我画了一张结构图。你可以把它当作本章的“地图”——后面每讲一个知识点，你都能在这张图上找到它的位置。

学位姿估计，别一上来就调参数、跑数据集。先把评价指标搞明白——你连“好”和“坏”都分不清，怎么优化？

我个人习惯是：拿到一个新算法，先跑一遍公开数据集（比如 EuRoC、TUM RGB-D），把 RPE 和 ATE 算出来，看看它到底在什么水平。然后再去读，看作者是怎么优化这两个指标的。这样学起来，效率高得多。

避坑指南：我曾经为了刷 ATE 指标，疯狂调后端优化参数，结果 RPE 反而变差了。后来才发现，ATE 和 RPE 有时候是矛盾的——你压低了全局漂移，可能牺牲了局部平滑度。所以，两个指标要一起看，别偏科。

好了，这一章就到这里。你只要记住三件事：位姿估计是啥、用在哪儿、怎么评价。后面所有章节，都是围绕“怎么让 RPE 和 ATE 更小”展开的。