1. 什么是SLAM？
SLAM是英文 Simultaneous Localization and Mapping 的缩写，中文全称为同时定位与建图。它主要解决两个核心问题：
1.1 定位：解决“我在什么地方？”
定位是指确定自身在环境中的具体位置。例如：你目前位于哪国、哪省、哪市、哪区、哪条路、哪栋楼、几号、几楼、哪个房间、哪个角落，都是定位的范畴。
1.2 建图：解决“周围环境什么样？”
建图是指获取环境的结构信息。你可以想象百度地图、高德地图，甚至景区的手绘地图——它们都描绘了环境的样子，建图就是生成类似的描述。
1.3 举例说明：扫地机器人
对于扫地机器人：
- 定位：知道自己在地板的哪个位置。
- 建图：了解整个房间的地面结构（如家具摆放、障碍物分布等）。
- 路径规划：基于定位和建图，以最短距离到达目标点。
下图展示了扫地机器人在室内移动时的定位与建图（蓝点代表机器人当前所在位置，已构建出可抵达区域的地图）：
1.4 SLAM的通用流程
机器人从未知环境、未知地点出发，通过传感器（本教程主要讨论相机）观测环境获取信息，利用相机信息估算自身的位置、姿态和运动轨迹，同时根据位姿构建地图，从而实现同时定位和建图。
目前主流的传感器类型包括：
- 激光雷达（LiDAR SLAM）
- 相机（Visual SLAM，即视觉SLAM）
- 惯性测量单元（Visual-inertial SLAM）
本教程重点介绍视觉SLAM，即只使用相机作为传感器的方法。相机类型包括：单目相机、双目相机、RGB-D相机。
小提示： SLAM的核心思想可以总结为“在移动中同步估计自身位置和构建环境地图”。想象你蒙上眼睛走进一个陌生房间，一边摸索墙壁一边在脑中创建房间布局——这就是SLAM的精髓所在。
2. 为什么需要SLAM？
你可能会问：为什么不用GPS？为什么不用现成的地图？下面我们来解答这些疑问。
2.1 为什么不用GPS？
- 无GPS场景：在建筑物内部、隧道、偏远山区等地方，无法获取GPS信号，机器人必须依靠自身定位。
- 精度不足：GPS定位精度通常只有几米。在室内环境中，一间房面积有限，几米的误差可能导致扫地机器人误以为自己跑到了邻居家。
2.2 为什么不用现成地图？
- 地图缺失：家庭、公司、工厂等场所通常难以获得现成图纸。每个家庭的装修、家具摆放各不相同，数据量庞大且无法通用。
- 地图需求多样：不同应用场景需要不同类型的地图（后面会详细介绍），不存在适用于所有问题的通用地图。
结论：在缺少GPS和现成地图的场景下，结合定位精度和地图需求，SLAM是机器人实现自主移动的关键技术。
3. 视觉SLAM如何实现？
下面以高博《视觉SLAM十四讲》中的经典框图来讲解视觉SLAM的整体框架：
流程如下：
- 传感器获取数据：相机采集一帧一帧的图像。
- 前端视觉里程计：通过图像信息计算相机的位置（准确来说是位姿，后面细说）。
- 闭环检测：判断机器人是否到达过之前经过的地方。
- 后端非线性优化：对前端计算的位姿进行优化，得到全局最优状态。
- 地图构建：根据每一时刻的相机位姿和空间目标信息，按需建立地图。
接下来，我们详细讲解每个模块。
4. 前端视觉里程计（VO）
视觉里程计（Visual Odometry，简称VO）主要研究如何通过相邻两帧图像计算相机之间的运动。它涉及图像处理（特征提取与匹配）、三维视觉（刚体运动、对极几何）、数学（李群李代数）等知识。别担心，我们逐步分解。
4.1 特征点法与直接法
视觉SLAM前端按方法分为特征点法和直接法。本节介绍特征点法的流程：
- 特征提取：在图像中找到“特别”的地方，如角点、边缘点。
- 特征匹配：在两张图像之间找到对应的特征点。
- 剔除误匹配：筛选掉匹配错误的对。
- 计算相机位姿：利用匹配点对估算相机运动。
4.2 什么是位姿？
位姿是位置和姿态的合称：
- 位置：三维空间中的坐标 (x, y, z)。
- 姿态：三维空间中的旋转 (roll, pitch, yaw)，共6个自由度。
为了直观理解姿态，可以做个头部健康运动：
- 左右歪头 — 滚转角 (roll)
- 上下点头 — 俯仰角 (pitch)
- 左右摇头 — 偏航角 (yaw)
4.3 常见坐标系
在相机运动过程中，涉及四个常用坐标系，下图清晰展示了它们的关系：
- 世界坐标系：三维空间的全局坐标系，原点可自行定义（如桌子一角或相机起始位置）。
- 相机坐标系：以相机光心为原点，光轴为z轴。
- 归一化坐标系：原点在相机坐标系下 (0,0,1) 处的二维平面坐标系。
- 像素坐标系：以图像左上角像素为原点，以像素为最小单位的离散坐标系。
4.4 坐标系变换
已知点在一个坐标系下的坐标，如何得到它在另一个坐标系下的坐标？例如：
- 通过相机投影模型，将像素坐标（特征点）转换到相机坐标系。
- 要构建地图，还需将相机坐标系下的坐标转换到世界坐标系。
下图展示了世界坐标系到相机坐标系的转换（其他转换同理）：
整篇文章唯一一条数学公式如下：
- pc：点p在相机坐标系下的坐标。
- pw：点p在世界坐标系下的坐标。
- Rcw：从世界坐标系到相机坐标系的旋转矩阵（3×3），决定姿态。
- tcw：从世界坐标系到相机坐标系的平移向量（3×1），决定位置。
相机的位姿就是相机在世界坐标系下的位置坐标和旋转姿态。位姿估计就是根据两帧匹配点计算当前时刻相机的位姿，如下图所示：
例如：将第1帧时刻的相机作为世界坐标系原点，通过第1、2帧图像匹配点可计算旋转矩阵R12和平移向量t12；再通过2、3帧计算R23和t23，结合之前结果即可得到第3帧位姿，以此类推。
4.5 计算R和t的三种情况
| 情况 | 方法 | 适用相机 | 说明 |
|---|---|---|---|
| 2D-2D | 对极约束 | 单目相机 | 只有二维图像，利用对极几何求本质矩阵E，再得R和t。 |
| 3D-3D | ICP（迭代最近点） | 双目或RGB-D相机 | 已知深度，直接求两个相机坐标系的转换。 |
| 3D-2D | PnP | 任意相机 | 已知三维点在世界坐标系下的坐标及在相机中的投影，直接求当前位姿。 |
小提示： 如果对公式感到头疼，暂时可以只记住：旋转矩阵R决定“朝哪转”，平移向量t决定“挪多远”。实际使用中，大多依赖现成库（如OpenCV）完成计算。
5. 后端优化
视觉里程计计算的位姿会存在误差，每次累积后误差会越来越大（称为累积误差）。后端优化就是为了消除这些误差，得到最优位姿。
后端有两种主流方法：
- 滤波器方法：以扩展卡尔曼滤波（EKF）为代表，假设当前状态只与上一状态有关。
- 非线性优化方法：以高斯牛顿法、列文伯格-马夸尔特法为代表，考虑当前状态与所有历史状态相关。
SLAM中主要使用非线性优化，对相机位姿和路标点构建最小二乘问题，并通过图优化求解，即Bundle Adjustment（BA）。由于BA处理的数据量很大，常采用滑动窗口法控制优化数据量：假设每次只优化10帧，当接收到第11帧时，移除第1帧数据，加入第11帧数据。
6. 回环检测（Loop Closure）
回环检测让机器人识别出曾经去过的地方。随着时间推移，误差累积会导致轨迹漂移。通过检测到回环，机器人利用历史数据修正累积误差，获得全局一致的状态估计。
回环检测本质上是一个图像识别问题，判断“两张图像是否为同一个地方”会有四种结果：
- 事实是回环，机器人判断为是 → 正确
- 事实是回环，机器人判断为否 → 漏检
- 事实不是回环，机器人判断为是 → 误检
- 事实不是回环，机器人判断为否 → 正确
衡量效果的指标有两个：
- 准确率（Precision）：所有被判断为回环中，真正是回环的比例。
- 召回率（Recall）：所有真正的回环中，被正确判断出来的比例。
在SLAM中，更注重召回率，希望更多真实回环被检测出来（宁可误检也不少漏）。传统视觉SLAM采用词袋模型进行回环检测。此外，回环检测还可用于重定位，在跟踪失败时快速恢复当前精确位姿。
7. 地图构建
地图构建根据传感器类型和应用需求产生不同类型的地图。高博《视觉SLAM十四讲》中的图清晰地说明了不同场景对应的地图类型：
- 纯定位：只需知道位置，使用稀疏路标地图即可。
- 导航、避障：机器人需要知道可通行区域和障碍物，需稠密地图（稀疏路标无法判断物体是什么）。
- 重建：需要带有轮廓、纹理等详细信息，使用稠密地图。
- 人机交互：需知道物体的语义信息（如桌子、杯子），使用语义地图。
7.1 八叉树地图（Octomap）
导航中常用的一种地图是八叉树地图，它通过递归划分三维空间来节省存储空间：
- 将三维空间分为多个方块。
- 每个方块再分为八个相同大小的小方块（称为子节点）。
- 如果某个方块的所有子方块都被占据或都未被占据，则不再继续细分。
相比点云地图，八叉树地图大幅减少了存储空间。下图是一个八叉树地图示例：
8. 常见问题解答（FAQ）
Q1：视觉SLAM和激光SLAM哪个更好？
答：各有优劣。视觉SLAM成本低（仅需摄像头），能获取纹理信息，但受光照影响大；激光SLAM精度高、不受光照影响，但成本高，且无法获取颜色纹理。选择取决于应用场景和预算。
Q2：单目相机为什么无法直接获取深度？
答：单目相机将三维场景投影到二维图像，丢失了深度信息。需要移动相机通过多帧图像（如对极几何）才能估计深度。双目相机和RGB-D相机则能直接或间接获取深度。
Q3：为什么要用非线性优化而不是滤波器？
答：滤波器（如EKF）只考虑当前状态与上一状态的关系，易产生累积误差；非线性优化考虑所有历史帧，精度更高，尤其适合大规模场景。但计算量更大，需滑动窗口等方法平衡。
Q4：回环检测失败会有什么后果？
答：如果没有回环检测，误差会无限累积，导致轨迹和地图严重漂移，机器人无法准确知道自己的位置。回环检测是消除累积误差的关键。
Q5：学习视觉SLAM需要哪些基础？
答：建议熟悉以下内容：线性代数（矩阵运算、空间变换）、图像处理（特征提取）、三维视觉基础（相机模型、对极几何）。编程方面掌握C++和ROS（机器人操作系统）更佳。推荐从《视觉SLAM十四讲》开始系统学习。
视觉SLAM领域广袤且持续发展，从入门到精通需要大量的实践和理论积累。希望本教程能为你提供一个清晰的起点。