拿手机围着办公桌走一圈视频拍下来了但手机能不能只靠这段视频搞清楚自己走了多远、转了几个弯甚至把桌子的轮廓画出来这就是 Visual SLAM 最朴素的问题也是我入坑三年多来觉得最值得反复琢磨的一件事。Visual SLAMVisual Simultaneous Localization and Mapping的核心就一句话让设备在从没见过的环境里靠摄像头拍到的图像一边确定自己的位置一边构建环境地图。它已经是扫地机器人、无人机避障、AR 眼镜、自动送货小车里的常客也是自动驾驶感知方案里不可缺少的补充模块。这篇文章本质上是一篇“算法笔记”我尽量用自己的话把 Visual SLAM 的主线逻辑讲清楚。不敢说面面俱到但我会把从坐标系怎么定义到后端优化在优化什么再到”开源方案怎么选、跑起来有哪些坑“完整串一遍。适合三类人看一是刚接触 SLAM 的研究生需要一个不绕弯的全局视野二是准备在项目中引入视觉定位能力、但还没决定用哪个方案的工程师三是自己刷过一些论文但总觉得代码和理论对不上号的爱好者。看完之后你会发现SLAM 并不是一个黑盒它的每一块都是可拆解、可复现、可调优的。1. 入坑前必须想明白的四件事1.1 坐标系的定义一切定位问题的地基Visual SLAM 里所有的计算都在回答一个问题相机在哪里、朝哪个方向看、周围有什么。这三个问题分别对应三个坐标系世界坐标系决定“周围有什么”的参考基准相机坐标系跟着镜头走像素坐标系则是我们真正能从图像里拿到的数据。先捋一下成像的完整链路。假设世界坐标系里有一个三维点P它在相机坐标系下的坐标是P_c那么这个点会通过针孔模型投影到像素平面形成像素坐标p (u, v)。这个过程的数学表达式可以拆成两步先把P从世界坐标系变换到相机坐标系这一步用到相机的外参T_wc包含旋转矩阵 R 和平移向量 t再把相机坐标系下的点投影到像素平面这一步用到相机的内参矩阵 K。所以整个投影过程就是p K * T_wc * P这个公式看起来平淡无奇但它解释了 SLAM 里两个最关键的操作前端做的事情是反过来从连续两帧图像里的匹配点p1、p2反推出相机运动T后端做的事情则是给定多个帧的运动估计和观测值反过来优化三维点P的位置和相机轨迹。所有前端、后端、回环的代码本质上都在反复折腾这个公式的顺推和反推。这也是为什么我建议初学者不要跳过坐标变换直接看算法。把R、t、K、畸变参数这几个符号彻底搞清楚后面看 ORB-SLAM 的代码你会发现 90% 的函数调用都是在做坐标系的来回切换。1.2 旋转怎么表示从旋转矩阵到李代数旋转矩阵是 3x3 的9 个数里面只有 3 个自由度并且要满足正交约束和行列式为 1。这意味着如果用旋转矩阵做优化每次迭代都要重新施加约束非常麻烦。所以 SLAM 里普遍用李代数来表述旋转和位姿最常见的是so(3)和se(3)对应的旋转向量和变换向量。简单说李代数允许我们把旋转表示成一个三维向量不加任何约束可以像普通向量一样求导、累加。等要实际使用的时候再通过指数映射把它变回旋转矩阵。这个思想在整个后端优化里是核心中的核心高斯牛顿法、列文伯格-马夸尔特法对位姿求导时雅可比矩阵都是定义在李代数上的。刚接触的时候我一度觉得李代数是为了把简单事情搞复杂。直到自己写了一个纯用旋转矩阵做优化的位姿图每次迭代都要手工做正交化误差总是降不下去才体会到李代数的价值。它的本质不是增加数学难度而是把带约束的优化问题转换成无约束优化解放了后端求解器的自由度。1.3 相机内参和畸变先标定再谈定位很多入门项目跑不起来或者轨迹飘第一嫌疑就是相机内参不对。内参矩阵 K 包含焦距fx、fy和主点cx、cy畸变系数通常包含径向畸变k1、k2、k3和切向畸变p1、p2。如果拿未标定的相机跑 ORB-SLAM初始化阶段可能直接失败或者轨迹漂移大到完全没法看。我个人做法是拿到新相机第一步就做标定用棋盘格拍 20-30 张不同角度的照片跑一遍 OpenCV 的标定接口或者 Kalibr 工具。标定一次不到半小时省下的是后面排查问题的无数时间。需要特别注意的一点是如果你用的是鱼眼镜头或广角镜头普通针孔模型的标定效果会很差这时候要考虑使用equidistant或fisheye模型别硬套针孔模型。1.4 状态估计的数学本质最大后验估计SLAM 归根结底是一个状态估计问题。我们把相机的轨迹设为x_1, x_2, ..., x_n把观测到的路标点设为l_1, l_2, ..., l_m那么整个 SLAM 就是给定一系列观测z和控制输入u的条件下求解最可能的轨迹和地图点这本质上是一个最大后验估计MAP问题。在假设观测噪声为高斯分布之后MAP 问题会等价于一个非线性最小二乘问题。也就是说我们要最小化所有误差项的平方和——包括相邻帧之间的运动误差、相机观测到路标点的重投影误差、回环检测带来的约束误差。这就是为什么后端优化里到处都在用高斯牛顿法、LM 算法、Dogleg 算法它们都是用来加速求解这个大规模稀疏最小二乘问题的工具。理解了这一层你就明白为什么 SLAM 系统总是“前端给猜测、后端做微调”的模式前端速度快但精度有限后端精度高但计算量大两者配合才能兼顾实时性和准确性。2. 一套完整 SLAM 系统的四块拼图2.1 前端视觉里程计帧与帧之间发生了什么前端视觉里程计的任务是估计相机在相邻帧之间的相对运动。主流有两种路线特征点法和直接法。特征点法的思路是先从图像里提取关键点和描述子比如 ORB、SIFT、SURF。然后对相邻两帧做特征匹配得到一组 2D-2D 匹配点对。有了匹配点对之后可以通过对极几何求解本质矩阵 E 或基础矩阵 F再从 E 中分解出旋转 R 和平移 t。如果场景里有已知的三维结构比如之前已经三角化过的路标点还可以用 PnP 方法求解帧间运动。直接法的思路则完全不同它不提取特征点而是假设同一个三维点的像素灰度在不同帧里保持不变灰度不变假设然后通过最小化像素灰度差来联合估计相机位姿和点的深度。DSO 和 LSD-SLAM 都是直接法的代表。直接法在纹理稀少的环境中往往比特征点法更鲁棒但对光照变化非常敏感而且对图像质量要求更高。在实际工程中特征点法仍然占据主导地位。ORB-SLAM 系列、VINS-Mono 的视觉部分、以及很多商业方案都基于特征点。原因很简单特征点匹配天然具备尺度不变性和旋转不变性ORB 描述子计算速度也够快容易做到实时。直接法更多用在需要极快运行速度或者弱纹理场景的特定场景里。2.2 为什么要做后端优化前端估计是近视眼前端每次只估计相邻两帧之间的运动误差会一帧一帧累积这就是“漂移”的来源。假设每帧估计有 0.5% 的平移误差走 100 米就可能漂移几十厘米甚至更多。后端优化的作用就是站在全局的角度把一段历史窗口内的所有约束放在一起联合优化把漂移拉回来。后端有两个关键机制关键帧和局部地图。如果每一帧都做优化计算量会爆炸所以 SLAM 系统会挑出那些信息量大的帧作为关键帧忽略掉大部分重复度高的普通帧。局部地图则维护当前关键帧能看到的那些路标点。当新关键帧加入时系统会在一个局部窗口内联合优化关键帧位姿和局部地图点位置这就是局部 Bundle AdjustmentBA。如果要讲得再细一点后端优化的对象通常是一个“因子图”或者“位姿图”节点表示关键帧的位姿和地图点边表示帧与帧之间的运动约束、观测约束、回环约束。优化过程就是在调整所有节点的参数让所有边的误差整体最小。听起来复杂但如今有 g2o、GTSAM、Ceres 这些现成库可以直接调用我们只需要定义好节点和边的类型求解器会自动完成迭代过程。2.3 回环检测让系统认出来过的地方回环检测是消除长期漂移的最有效手段。它的逻辑很直接如果相机回到了之前访问过的区域系统要能认出来。一旦回环被确认就可以在当前位置和历史上的某个关键帧之间建立一条约束然后通过全局优化把整个轨迹“拉”回正确位置。视觉回环检测的主流实现是词袋模型Bag of Words。过程大致是离线阶段对大量图像提取特征描述子用 K-means 聚类得到视觉词汇构建一个“视觉词典”在线阶段把当前帧的特征映射到词典中的词汇形成一个直方图向量然后比较当前帧和历史关键帧的直方图向量之间的相似度超过阈值就认为是回环候选。这里有一个我踩过很多次的坑词袋模型对视角变化和光照变化比较敏感。同一个走廊晚上灯光全开和白天自然光下直方图差异会很大。提升鲁棒性的常见做法包括用更紧凑的二值描述子DBoW2 配合 ORB或者加入几何一致性验证——候选回环必须通过特征匹配和本质矩阵分解的验证之后才会被接受。ORB-SLAM3 的 Loop Closing 模块里就包含了从词袋检索到 Sim3 求解、再到图优化的完整流程这一套逻辑非常值得精读。2.4 建图稀疏点云还是稠密地图定位和建图在 SLAM 里是相辅相成的但“地图”到底长什么样取决于下游任务。如果你只需要知道相机轨迹和稀疏路标点稀疏点云就够了ORB-SLAM 默认输出的就是这种。如果你做机器人导航稀疏点云没法用于避障需要八叉树地图OctoMap或栅格地图。如果你做 AR 或者三维重建则需要半稠密或稠密深度图。不同建图策略对应不同的算法选择。LSD-SLAM 直接对图像中梯度明显的像素估计逆深度能输出半稠密地图DTAM、KinectFusion 这类方案用体素或深度图融合的方式生成稠密重建但计算量更大而且需要 RGB-D 相机或较强的 GPU。选择哪条路线先想清楚最终产品需要什么样的地图再反过来选系统。3. 主流开源方案怎么选从 ORB-SLAM3 到 VINS-Fusion3.1 ORB-SLAM3目前最值得研究的特征点法全家桶ORB-SLAM3 是西班牙萨拉戈萨大学团队发布的视觉 SLAM 系统支持单目、双目、RGB-D并且集成了 IMU 惯性单元支持视觉-惯性融合。它继承了 ORB-SLAM2 的三大线程跟踪、局部建图、回环架构增加了地图融合和重定位机制还能管理多个子地图Atlas。在学术圈和工业界ORB-SLAM3 已经算事实上的基线系统。它最大的优点是工程实现极其完整论文里的每个模块都对应着具体代码。我的建议是想理解特征点法 SLAM 的完整链路花两周时间通读 ORB-SLAM3 的代码比刷十篇论文管用。它有明显的局限依赖特征点在弱纹理场景白墙、天空、水面容易跟踪丢失纯单目模式下需要足够的视差才能启动初始化对动态物体没有专门处理行人多的地方轨迹容易被干扰。3.2 DSO 与 LSD-SLAM直接法的两个代表DSODirect Sparse Odometry由慕尼黑工业大学团队提出是稀疏直接法的巅峰之作。它不提取特征点而是选择图像中梯度明显的像素点直接优化像素灰度残差。DSO 在计算效率和精度上表现优异在 EuRoC 数据集上甚至能超过同期的特征点方案。但它是一个里程计没有回环检测漂移问题需要靠外部方法弥补。LSD-SLAM 是半稠密直接法的代表它的核心思想是对图像中有梯度的像素估计逆深度允许构建半稠密地图。相比 DSOLSD-SLAM 更加“轻”但精度和稳定性略逊一筹。直接法的通病是灰度不变假设在真实光照变化剧烈的环境中很容易失效所以用在室内固定光照场景还行室外大太阳底下就很吃力。我的经验是直接法适合当“兴趣研究”或者用在专门的弱纹理室内场景里。做产品还是踏踏实实用特征点方案或者视觉-惯性融合方案稳定性会高很多。3.3 VINS-Mono 与 VINS-Fusion视觉惯性融合的标准答案纯视觉 SLAM 有一个绕不过去的短板快速运动导致图像模糊或大视角变化时特征跟踪容易丢失。融合 IMU 可以大幅缓解这个问题。IMU 的输出频率通常在 100-200Hz高于相机的 30fps通过预积分可以在两帧图像之间提供短时间内的运动预测弥补图像的不足。VINS-Mono 和它的后续版本 VINS-Fusion 是视觉惯性融合里最经典的开源方案。它采用基于滑动窗口的优化框架维护一组最新的关键帧联合优化它们的位姿、速度、偏置和路标点。VINS-Fusion 还支持 GNSS 和全局定位模块的接入便于在室外大场景下长期运行。选型时有一个倾向性建议如果项目要求从零搭建一个能跑真机的定位系统VINS-Fusion 的上手成本比 ORB-SLAM3 低不少因为它的代码结构更清晰、依赖更少而且对 IMU 的接入方式做了很好的封装。如果你的主要目标是做学术对比或者研究回环检测那 ORB-SLAM3 的地图管理机制更值得深挖。3.4 选型对比不同场景的推荐方案方案输入回环惯性融合建图能力适合场景ORB-SLAM3单目/双目/RGB-D有支持稀疏点云学术基线、多数室内/室外有纹理场景VINS-Fusion单目/双目IMU有(基于DBoW)核心特性稀疏点云无人机、移动机器人、手持设备DSO单目/双目无支持(VI-DSO)稀疏(带深度的点)关注里程计精度、弱纹理场景LSD-SLAM单目/双目有(弱)无半稠密实时半稠密建图研究RTAB-MapRGB-D/双目有支持稠密(3D网格)室内机器人和 AR 重建这张表是决策参考不是教条。实际选型还要看你手头的传感器、算力平台、系统复杂度预期。我个人做过的一个判断是如果项目只有单目相机且算力有限优先考虑 ORB-SLAM3 的单目模式如果有一块好的 IMU 并且场景中有快速运动VINS-Fusion 更稳如果要输出稠密地图给导航用RTAB-Map 比前几个省事得多。4. 跑开源 SLAM 时最常踩的坑与排查链路4.1 相机标定不准整个系统都会“萎”有次我拿一个没标定过的 USB 摄像头跑 ORB-SLAM3启动倒是正常但相机一转动地图点就马上发散轨迹直接飞出天际。查了半天 OpenCV 版本、编译参数最后发现就是内参有问题。换回标定好的参数问题当场消失。标定采集图像时有几个细节容易忽略棋盘格要贴平不能有褶皱照片要覆盖整个视野尤其是画面边缘光照要均匀最好用漫射光源避免反光斑。标定结果的评判标准是重投影误差一般低于 0.3 像素算合格。如果误差偏大检查图像数量够不够、棋盘格角点检测是否准确以及镜头是否严重畸变却用的是针孔模型。4.2 单目初始化失败的完整排查链路单目 SLAM 初始化需要两帧之间存在足够的视差才能通过三角化恢复三维结构。常见失败场景是这样的相机对着白墙平移特征点太少匹配不上或者相机静止不动没有视差或者相机在快速旋转两帧之间重叠区域太小。遇到初始化失败我的排查顺序是先看当前帧特征点数量够不够ORB-SLAM3 默认每帧提取 1000 个左右如果只剩两三百个说明场景纹理太弱再看匹配点对的分布是否均匀如果全部集中在画面中央也会影响本质矩阵分解的稳定性最后确认两帧之间的平均视差是否在合理范围太低就引导用户缓慢平移太高可能是匹配错误太多。如果想快速验证是不是“视差问题”拿一套带纹理的书架或者绿植场景测一下如果立刻能初始化成功那问题就锁定在场景条件上。4.3 时间戳不同步视觉惯性系统的隐形杀手跑 VINS-Fusion 的时候最崩溃的莫过于轨迹前几分钟还好好的突然位姿开始剧烈跳动。后来发现问题出在时间戳上相机和 IMU 的时钟没有对齐IMU 数据晚到了几十毫秒。几十毫秒在视觉里不算什么但在 200Hz 的 IMU 积分下足以让预积分结果产生明显偏差。排查方法是分别记录相机和 IMU 的采样时间戳画出时间差曲线。如果时间差是固定值可以用time offset补偿如果是漂移的说明两个传感器没有同一时钟源需要在硬件上做硬件触发同步或者用软件在线估计时间偏移VINS-Fusion 里有估计时间偏移的选项。这一步在真机部署时特别重要仿真环境里通常不会暴露。4.4 数据集高分不等于真机可用很多初学者都会犯一个认知错误算法在 EuRoC、KITTI 上跑出来的轨迹精度很高就认为可以直接装到真实产品上。但数据集的场景通常光照稳定、纹理丰富、相机运动模式单一。真实场景面临的是变化的光照、动态行人、反光地板、昏暗走廊算法的表现会大打折扣。我的建议是评估方案的时候先在自己的业务场景里采集一段数据做一个“伪真机测试”。轨道精度到底怎么样直接在录好的数据上跑一遍离线 SLAM 就知道了。千万别等到真机联调才发现方案选错了那时候改型的成本是巨大的。5. 从跑通到真正理解一条更靠谱的学习路径5.1 先跑通再读理论不要反着来我在学习初期犯过一个严重错误花了两个星期啃《多视图几何》里对极几何和三角化的证明却连一段数据集都没有跑过。后来在 ORB-SLAM3 里跑了 TUM 数据集看到地图点和轨迹出现在可视化界面上才真正把书里的符号和实际系统对应起来。建议路线是第一天就编译运行 ORB-SLAM3 或者 VINS-Fusion把 TUM 和 EuRoC 数据集各跑一遍感受一下轨迹、点云、关键帧的概念然后再回到《视觉SLAM十四讲》或者相关论文里把不清楚的公式逐个击破。有了直观感受之后再读理论你会不断产生“原来这里对应的是那一步”的顿悟。5.2 做消融实验是理解系统最快的方式所谓消融实验就是故意关掉系统的某些模块观察整体表现如何退化。你可以在 ORB-SLAM3 的代码里把回环检测线程停止跑同一条数据集对比轨迹的漂移程度也可以把局部 BA 的迭代次数改成 0看看前端跟踪会不会马上发散。通过这些操作你会直观理解每个模块存在的意义。我之前把这套“破坏性实验”用在 VINS-Fusion 上把 IMU 因子完全禁掉只保留视觉部分轨迹在快速转弯的地方立刻开始飘。这次实验比读十页论文更深刻地让我理解了 IMU 的作用它负责在一段图像跟踪丢失期间保持系统的“记忆”。5.3 读代码的优先级先数据流后推导面对一个大型 SLAM 开源工程不要试图从上到下顺序读完。我会先找数据流入口图像从哪个回调函数进入特征提取发生在哪里关键帧何时被创建地图点何时被加入把这些“事件流”串起来之后再顺着数据流去读具体的算法函数这样脑海中会形成一颗清晰的调用树。以 ORB-SLAM3 为例建议顺序是先读Tracking线程的Track()函数看一帧图像进来之后依次经过哪些步骤再读LocalMapping里的ProcessNewKeyFrame()看新关键帧如何被处理最后再看LoopClosing的回环检测和全局 BA。顺序千万别反直接先读Optimizer.cc会很快迷失在数学细节里。5.4 学习过程中常见的三种错误心态第一是“收藏等于学会”。下了三十个开源库收藏了五十篇论文却一个都没真正跑通这是新手最常见的问题。第二是“只看论文不写代码”。论文是基于理想条件的抽象写代码才是把抽象还原为具体的过程。第三是“只调参数不思原理”。调参能解决眼前的问题但解决不了系统设计的瓶颈。这些年我带过不少人入门 SLAM进步最快的往往是那些愿意把“跑不通”的报错反复读三遍、愿意自己画数据流图、愿意写小实验去验证疑问的人。SLAM 的门槛不在于数学有多深而在于能不能把一堆符号和概念重新组织成一个能跑的闭环。走通这个闭环之后无论你是做机器人、自动驾驶还是 AR视角都会完全不同。
