SLAM 从视觉里程计到建图（6）

前言

读到这里，你已经掌握了视觉 SLAM 的全部数学工具：位姿如何表示（第 3 讲），位姿如何求导和更新（第 4 讲），三维点如何投影成像素（第 5 讲），有了残差和雅可比之后如何优化（第 6 讲）。接下来的八讲不再引入新的数学，而是把这些工具组装成一个能跑的系统。

组装的顺序和第 2 讲画的框架图一致：前端 （第 7、8 讲）从相邻两帧图像估计相机运动，后端 （第 9、10 讲）把一段时间内的所有运动和路标放在一起优化，回环检测 （第 11 讲）发现相机回到了去过的地方，建图（第 12 讲）把估计结果变成有用的环境模型，最后（第 13 讲）把这些模块写成一个真实的程序，（第 14 讲）看看别人是怎么做的、未来会怎样。

这一册的写法和前四册不同。前四册是"精读"，每个公式都推；这一册是"导读"，每一讲只回答三个问题：它解决什么问题、核心思想是什么、有哪几条结论必须记住。目的是让你在合上书之后，脑子里有一张完整的地图，知道每个零件在哪、为什么需要它。需要细节时再回到书的对应章节和本目录的论文。

第一部分　书的后半本：八讲全景

第 7 讲　视觉里程计 1：特征点法

解决什么问题。 给两张相邻的图像，估计相机在这两帧之间动了多少（一个 SE(3) 变换），顺便估计看到的点在三维空间的位置。这是 SLAM 前端的核心任务，叫视觉里程计（Visual Odometry，VO）。

**核心思想。**先在两张图上各找一些"有特点、容易认出来"的像素点（特征点），再找出哪些点是同一个三维点在两张图上的投影（匹配），最后用这些匹配对解一个几何问题得到相机运动。

特征点与描述子。 一个好的特征点要在相机运动、光照变化、视角变化下仍然能被认出来。书用 ORB （Oriented FAST and Rotated BRIEF）：用 FAST 角点检测找到亮度变化剧烈的点，加上图像金字塔解决尺度问题、用灰度质心法算出方向解决旋转问题；然后在每个点周围按预设模式比较 256 对像素的亮度大小，得到一个 256 位的二进制串作为描述子。两个描述子的相似度用汉明距离（不同位的个数）衡量，可以用 CPU 的位运算指令极快地算出来。ORB 是精度和速度的平衡点------SIFT 更稳但慢十倍以上，ORB 能在普通 CPU 上实时跑，ORB-SLAM 全系列用它。

**匹配。**对第一张图的每个描述子，在第二张图里找汉明距离最小的。暴力匹配是 O(n²)，可以用 FLANN 这类近似最近邻加速。匹配结果里一定有错配，常用的筛选是"最小距离的两倍"或者 Lowe 比率测试，但最终还得靠下面的几何方法（RANSAC）来剔除。

**三种几何问题，按已知信息分类。**这是第 7 讲最重要的结构，一定要记住：

已知	问题	方法	适用场景
2D-2D：两张图的像素匹配，没有深度	对极几何	八点法求本质矩阵 E 或基础矩阵 F，分解出 R、t	单目初始化
3D-2D：一组三维点及其在图像上的投影	PnP（Perspective-n-Point）	DLT、P3P、EPnP，或 BA 迭代	单目后续帧、双目/RGB-D 全部帧
3D-3D：两组三维点的匹配	ICP（Iterative Closest Point）	SVD 闭式解，或 BA 迭代	RGB-D、激光

对极几何的关键结论。 一对匹配点 p₁、p₂（归一化坐标）和两帧之间的运动 R、t 满足对极约束：

p₂ᵀ t^ R p₁ = 0

t^ R 记作本质矩阵 E（3×3，5 个自由度）。至少 5 对点可以解 E，实用的是八点法（线性解）。E 分解出 R、t 有四个解，用"点在两相机前方"筛出唯一的一个。致命缺陷：t 只能恢复方向、不能恢复长度 ------这就是单目的尺度不确定性，通常把初始化时的 t 归一化成 1，之后所有尺度都相对于它。另一个坑：如果场景是一个平面（或相机纯旋转），E 退化，要换成单应矩阵 H；ORB-SLAM 同时算 E 和 H、按得分选。

三角测量。 知道 R、t 和一对匹配点，两条射线在空间中（近似）相交于一点，就是这个点的深度。结论：平移越大、深度越准；平移为零时根本无法三角化（这就是为什么纯旋转对单目是灾难）。平移和精度之间还有矛盾------平移大了，图像变化也大，匹配变难。

PnP 的关键结论。 知道 n 个三维点和它们的像素，求相机位姿。P3P 用三个点加一个验证点有闭式解；EPnP 更稳。但书最强调的是用 BA 解 PnP ：把位姿当作优化变量，最小化重投影误差------三维点按当前位姿投影到图像上、与实际观测像素之差：

e = p_uv − (1/Z) K (R P + t)

它对位姿的雅可比就是第 5 讲预告的那个 2×6 矩阵（像素对相机系点的 2×3 乘以相机系点对位姿的 3×6）：

                  ┌  fx/Z′    0      −fx X′/Z′²    −fx X′Y′/Z′²     fx + fx X′²/Z′²    −fx Y′/Z′ ┐
  ∂e/∂δξ  =  −    │                                                                            │
                  └   0      fy/Z′   −fy Y′/Z′²    −fy − fy Y′²/Z′²   fy X′Y′/Z′²        fy X′/Z′  ┘

（X′、Y′、Z′ 是变换后的相机系坐标；前三列对平移，后三列对旋转。）这个矩阵是整本书的"高光公式"------第 9 讲的 BA 直接用它。

**ICP 的关键结论。**两组三维点配对已知时，先各自去质心，对去质心后的点算一个 3×3 矩阵 W = Σ qᵢ q′ᵢᵀ，对 W 做 SVD 得 W = UΣVᵀ，则 R = UVᵀ，t 由质心差算出。这个闭式解就是 Kabsch / Umeyama 算法------你在机械臂标定里用过的刚体配准和这里是同一个东西。配对未知时才需要"迭代最近点"：先按当前位姿找最近点作为配对，解 R、t，再找、再解，循环。

配套论文。 10_ORB-SLAM：特征点法 SLAM 的标准架构。读它第 III~VI 节，看一个完整系统如何在"跟踪线程"里用 PnP + 局部地图做 VO。

第 8 讲　视觉里程计 2：直接法

**解决什么问题。**特征点法有三个毛病：提取和匹配特征耗时（一帧可能要 20 ms 以上）；只用了几百个点，图像绝大部分信息被丢弃；在弱纹理（白墙、走廊）处找不到特征。直接法试图不提特征、不做匹配，直接用像素亮度来估计运动。

**核心思想：光度不变假设。**同一个三维点在相邻两帧上的像素亮度应该相同。如果运动估计是对的，把第一帧的像素按这个运动投影到第二帧，两边亮度应该一致；不一致就说明运动估计有误，用亮度差作为残差去优化运动。

光流是直接法的前身。 光流（Optical Flow）追踪像素在图像平面上的二维运动，不涉及相机位姿。Lucas-Kanade 光流假设一个小窗口内所有像素运动一致，把亮度差对位移做泰勒展开，得到一个线性方程，解出位移。用图像金字塔从粗到细可以处理大位移。OpenCV 的 calcOpticalFlowPyrLK 就是它。光流的用途：代替描述子匹配------用光流追踪特征点，比算描述子快，VINS-Mono 前端就这么做。

直接法则直接优化位姿。 残差是光度误差：

e = I₁(p₁) − I₂( π( T · π⁻¹(p₁, d₁) ) )

第一帧像素 p₁ 带深度 d₁，反投影成三维点，用待估计的 T 变到第二帧，再投影成 p₂，比较两处亮度。它对 T 的雅可比是三项的链式乘积：图像梯度 （亮度对像素位置）× 投影雅可比 （像素对相机系点，第 5 讲）× 位姿雅可比（相机系点对位姿，第 4 讲）。前两讲的两个雅可比在这里第三次登场。

**按用多少像素分三类。**稀疏直接法（只用几百个关键点，不要描述子）、半稠密（用所有梯度明显的像素）、稠密（用全部像素，需要 GPU）。

关键结论与局限。 直接法在弱纹理处比特征法稳、速度快、能建半稠密地图；但它依赖光度不变假设 ，相机曝光变化、光照变化都会破坏它，所以 DSO 专门做了光度标定；它对初值敏感 ，非凸性强，大运动时容易掉进局部极小；它不能做回环检测（没有描述子就没法做地点识别）。

配套论文。 13_DSO：直接稀疏里程计，直接法的集大成者。读它第 2 节的光度模型和第 3 节的滑窗优化。对照 ORB-SLAM 看，"特征法 vs 直接法"是视觉 SLAM 前端的两条主路线，各有赢的场合。

第 9 讲　后端 1：Bundle Adjustment

**解决什么问题。**前端每次只看相邻两帧，误差会累积。后端把一段时间内所有的相机位姿和路标点放在一起，用所有观测同时约束它们，得到全局更一致的估计。

**核心思想。**就是第 6 讲的最小二乘，变量是所有位姿 T₁...T_m 和所有路标 P₁...P_n，残差是每一次观测的重投影误差：

min Σ_i Σ_j ‖ z_ij − π( T_i, P_j ) ‖²_Σ

这个问题叫 Bundle Adjustment（BA，光束法平差），名字来自"调整从相机射出的光束让它们汇聚到路标上"。它在摄影测量学里有一百年历史，SfM（Structure from Motion）用它做离线三维重建。

**书的重点是：BA 为什么能实时。**变量数巨大（位姿 6m + 路标 3n，n 常常上万），增量方程的 H 是 (6m + 3n) 维方阵，直接解不可能。但 H 有特殊结构：

• 一条重投影边只连一个位姿和一个路标，所以 H 里只有"位姿-位姿对角块"、"路标-路标对角块"和少量"位姿-路标"块非零。

• 路标和路标之间没有直接约束 （没有边连接两个路标），所以路标-路标部分是对角块矩阵（每个 3×3 块独立），极易求逆。

把 H 按位姿和路标分块写成

        ┌  B    E  ┐ ┌ Δx_c ┐   ┌ v ┐
        │          │ │      │ = │   │        B：位姿块（6m×6m，稠密但小）
        └  Eᵀ   C  ┘ └ Δx_p ┘   └ w ┘        C：路标块（3n×3n，块对角）

Schur 消元（边缘化）利用 C 好求逆，先消掉路标：

( B − E C⁻¹ Eᵀ ) Δx_c = v − E C⁻¹ w

左边这个只有 6m 维（位姿数通常几十到几百）的方程先解出位姿增量，再回代算路标增量。B − EC⁻¹Eᵀ 叫Schur 补 ，它把路标的信息"折叠"进了位姿之间。这个技巧让 BA 的复杂度从随路标数立方增长变成了近似线性，是实时 BA 的基石。g2o 的 LinearSolverCholmod + BlockSolver_6_3 就是在做这件事；Ceres 的 SPARSE_SCHUR 同理。

鲁棒核函数。 最小二乘对外点（错误匹配）极其敏感------一个错配的残差平方可以大到主导整个目标函数。解决办法是把平方换成一个在大残差处增长变慢的函数，常用 Huber 核 ：残差小于阈值 δ 时是平方，大于 δ 时变成线性。它让外点的影响有界。g2o 里 edge->setRobustKernel(new g2o::RobustKernelHuber())，Ceres 里 new ceres::HuberLoss(1.0)。几乎所有实际系统都用。

配套论文。 04_Grisetti 图优化教程：把 BA 推广到一般的图优化，讲清 g2o 的实现思路。11_ORB-SLAM2 第 IV 节：一个实际系统里局部 BA 和全局 BA 各在什么时候跑。

第 10 讲　后端 2：位姿图与滑动窗口

**解决什么问题。**BA 虽然用了 Schur 消元，但路标数无限增长，时间长了还是跑不动。需要进一步精简。

滑动窗口 ：只保留最近 N 个关键帧，更老的帧边缘化 掉------不是简单扔掉，而是把它们对剩余变量的约束"折叠"成一个先验（又是 Schur 补）。VINS-Mono、DSO 都用它。坑在于边缘化产生的先验和后续新观测如果在不同的线性化点上展开，会导致信息矩阵的零空间被错误地填满（系统"以为"自己知道了本不可观的量），解决办法叫 FEJ（First Estimate Jacobian，首次估计雅可比）------对被边缘化过的变量，雅可比永远在第一次线性化的点上算。

位姿图 （Pose Graph）：更激进，把路标全部扔掉，只留位姿。相邻位姿之间的相对运动（由前端 VO 或回环检测给出）作为边：

e_ij = ln( T_ij⁻¹ T_i⁻¹ T_j )∨

即"实际的相对位姿"与"边上记录的相对位姿"之差，在 se(3) 上表示。优化变量只有位姿，图小得多，可以做很大范围的全局优化。它对位姿的雅可比涉及第 4 讲的 J_r⁻¹（伴随和右雅可比），书给了近似公式 J_r⁻¹ ≈ I + ½ φ\^ ρ\^; 0 φ\^。

位姿图是回环检测后做全局修正的标准工具：回环给出一条新边，连接当前帧和很久以前的某帧，位姿图优化把整条轨迹拉回一致。ORB-SLAM 的 Essential Graph 就是位姿图（在 Sim(3) 上，为了同时修正单目尺度漂移）。

配套论文。 14_VINS-Mono 第 VI 节：滑窗边缘化和 FEJ 的实际处理；12_ORB-SLAM3：Atlas 多地图下的位姿图。

第 11 讲　回环检测

解决什么问题。 前端和后端都只能约束"相邻"的帧。走了一大圈回到起点，累积误差可能已经几米，只有认出"这是我来过的地方"，加一条跨越很长时间的约束，才能把误差消掉。这件事叫回环检测 （Loop Closure Detection），它本质上是地点识别。

核心思想：词袋模型（Bag-of-Words，BoW）。把图像看作一袋"视觉单词"，忽略单词的位置，只看每个单词出现了几次，用这个频率向量代表图像。两张图的相似度就是两个向量的距离。

字典怎么来。 把大量训练图像的 ORB 描述子聚类（k-means），每个聚类中心是一个"单词"。为了快速查找用 k 叉树：先聚成 k 类，每类再聚 k 类，深度 d，叶子就是单词，共 k^d 个（DBoW2 默认 k = 10、d = 5，百万量级单词）。一个新描述子从根往下走 d 步就找到它的单词，复杂度 O(d) 而不是 O(k^d)。

相似度计算。 用 TF-IDF 加权：TF（词频，这张图里出现多的单词更重要）× IDF（逆文档频率，在所有图里都常见的单词不重要）。两张图的词袋向量用 L1 范数算相似度。

工程细节决定成败。 书反复强调：相似度的绝对值没意义，要和"相邻关键帧的相似度"做归一化；要防止感知混淆 （两个长得像的不同地方）；回环候选要时间连续性验证 （连续几帧都检测到回环才算数）和几何验证（用 PnP 或对极几何核对两帧之间真的能算出合理的相对位姿）。没有后两步验证的回环检测在实际中不可用。

配套论文。 10_ORB-SLAM 第 VII 节：回环检测、Sim(3) 求解和 Essential Graph 优化的完整流程。

第 12 讲　建图

**解决什么问题。**前面的"地图"只是一些稀疏路标点，够用来定位，不够用来避障、规划、交互。建图讲如何得到更稠密、更有用的环境表示。

单目稠密重建。 没有深度传感器，要为每个像素估计深度。思路是极线搜索 + 深度滤波 ：一个像素的深度未知，但它一定在另一帧的某条直线（极线）上；沿极线用块匹配（NCC）找最像的位置得到一个深度观测；由于单次观测噪声大，用高斯分布表示深度的不确定性，每来一帧就做一次贝叶斯更新（深度滤波器），多帧之后收敛。书用这个例子说明了稠密重建的三个困难：像素梯度小的地方匹配不可靠；仿射变换/遮挡让块匹配失效；逐像素的计算量巨大。结论是单目稠密重建理论可行、实际很难实时且精度有限。

RGB-D 稠密建图容易得多，有深度直接拼点云（第 5 讲的 joinMap）。但原始点云有三个问题：太大（一帧几十万点）、没有拓扑（不知道哪里是"空"的）、不能更新。所以常用两种更好的表示：

• 八叉树地图（OctoMap）：把空间递归分成八块，每块存一个"被占据的概率"，用对数几率（logit）更新；空区域合并成大块，省内存；可以直接用于避障和导航。

• TSDF（Truncated Signed Distance Function）：每个体素存"到最近表面的带符号距离"（表面内为负、外为正、截断在一个范围内）。多帧融合是对距离做加权平均。从 TSDF 用 Marching Cubes 提取等值面得到光滑网格。KinectFusion、ElasticFusion 系列用它，需要 GPU。

**地图的用途决定地图的形式。**定位用稀疏点，导航用八叉树/栅格，重建用 TSDF/网格，交互和理解需要语义。没有"最好的地图"，只有"对这个任务最合适的地图"。

配套论文。 24_R3LIVE：激光建几何 + 视觉上色的彩色稠密重建，是 2022 年多传感器稠密建图的代表。

第 13 讲　设计一个 SLAM 系统

**解决什么问题。**前面的实践都是单文件小程序。真正的 SLAM 是一个多线程、多模块、要长期稳定运行的工程。书用双目数据集（KITTI）从头写了一个精简的 SLAM。

工程框架。 书的设计分成几个类：Frame（一帧图像 + 位姿 + 特征）、MapPoint（路标）、Map（管理所有帧和路标）、Frontend（提特征、光流追踪、三角化、决定是否插入关键帧）、Backend（独立线程，对局部地图做 BA）、Viewer（显示）。前端和后端通过条件变量通信------前端插入关键帧后唤醒后端优化。

**几条工程原则。**基础数据结构用智能指针管理生命周期；多线程间共享的数据加互斥锁；参数放配置文件而不是硬编码；每个模块能单独测试；用 glog 打日志、gtest 写单测。这些不是 SLAM 专属，是任何 C++ 工程的常识，但对从单片机转过来的人尤其值得注意------SLAM 的 bug 大半不在数学而在并发和生命周期。

**运行结果。**在 KITTI 上大约能实时跑，轨迹有漂移（没有回环），这是预期之中的------它是一个 VO 而不是完整 SLAM。书留给读者的扩展题包括加回环、加 BA 的鲁棒核、换成直接法前端等。

第 14 讲　SLAM 的现在与未来

书写于 2019 年，列了当时的主要开源方案（MonoSLAM、PTAM、ORB-SLAM、LSD-SLAM、SVO、DSO、RTAB-MAP）和当时认为的未来方向（IMU 融合、语义 SLAM、深度学习）。这些判断大体是对的，但七年过去，具体格局变了很多。下一部分用 2026 年的眼光重新梳理。

---

第二部分　2026 年的 SLAM：哪些变了，哪些没变

2.1 没变的：数学基础就是数学基础

这本书前六讲的内容------SE(3)、李代数扰动、针孔投影、非线性最小二乘、Schur 消元、鲁棒核------没有一样过时 。2026 年最新的系统，无论用不用神经网络，后端照样在解 (H + λI)Δx = g，照样左乘 exp(δξ^) 更新位姿，照样对路标做 Schur 消元。学习型方法替换的是前端的"特征"和"匹配"，替换不了几何约束。所以这四册精读的内容是保值资产 ，而后半本书的具体算法（ORB、BoW、八点法）是会被迭代的工具------工具可以换，用工具的框架不会换。

2.2 变了的：视觉前端被深度学习重做了一遍

书里的 ORB + 暴力匹配 + RANSAC 这条前端流水线，2020 年以后被三类学习型方法逐步替代：

**学习型特征点与描述子。**SuperPoint（2018）用自监督方式训练一个网络同时输出关键点和描述子，比 ORB 更稳定、对光照和视角变化更鲁棒。DISK、ALIKED 等继续改进。它们的接口和 ORB 完全一样------输出点坐标和描述子向量------所以可以直接替换进 ORB-SLAM 的前端。

**学习型匹配。**SuperGlue（2020）用图神经网络 + 注意力机制做匹配，考虑了点与点之间的上下文，比"最近邻 + 比率测试"准得多。LightGlue（2023）把它做到实时。这两个一起让"错配"的问题从"靠 RANSAC 硬扛"变成了"前端基本不错配"。

**稠密匹配与端到端位姿。**LoFTR（2021）不提特征点，直接对两张图做稠密匹配；DUSt3R（2024）和它的后续 MASt3R（2024）更激进------输入两张图，网络直接输出两张图所有像素的三维点坐标（在同一坐标系下），相当于一步完成了匹配 + 三角化 + 相对位姿；VGGT（2025）把这个思路推广到多视角。这类方法在弱纹理、大视角变化下远胜传统方法，正在改写 SfM 和 SLAM 的前端。

**对你的意义。**书第 7 讲的几何（对极约束、PnP、三角化）仍然必须懂，因为学习型前端输出的匹配最后还是要喂进这些几何求解器；但"手写 ORB"和"调 FLANN 参数"这类技能的重要性下降了。如果今天从头写一个视觉 SLAM 前端，合理的选择是 SuperPoint/ALIKED + LightGlue 做匹配，再接传统的 PnP + BA 后端。

2.3 变了的：地图表示进入了神经隐式和高斯时代

书第 12 讲的点云、八叉树、TSDF 是"显式"地图------每个体素存一个值。2020 年 NeRF 出现后，出现了"隐式"地图：用一个神经网络表示整个场景，输入空间坐标和视角，输出颜色和密度。iMAP（2021）、NICE-SLAM（2022）、Co-SLAM（2023）把 NeRF 装进 SLAM，边跑边训网络，位姿和地图一起优化。好处是地图连续、可渲染、能补全看不到的地方；坏处是训练慢、不能实时、大场景扩展困难。

2023 年 3D Gaussian Splatting（3DGS） 用一堆带位置、协方差、颜色、不透明度的三维高斯球表示场景，渲染用光栅化而不是光线行进，速度比 NeRF 快两个数量级。SplaTAM、MonoGS、Gaussian-SLAM（都是 2024 年）把它装进 SLAM，第一次做到了实时的、可渲染的、照片级的稠密建图。到 2026 年，3DGS 已经成为稠密视觉 SLAM 建图的默认选项，取代了 TSDF 的地位。

对你的意义。 第 12 讲的八叉树和 TSDF 仍然是机器人导航和工业测量的主力（它们能直接告诉你"这里能不能走"，3DGS 做不到）；但如果目标是重建、数字孪生、AR/VR，应该直接学 3DGS。本目录的 SLAM_05 笔记有 NeRF/3DGS 的入门内容。

2.4 变了的：工业落地的主流是激光惯性，不是纯视觉

书是一本"视觉 SLAM"的书，但 2026 年在机器人、自动驾驶、测绘这些真正赚钱的场景里，纯视觉 SLAM 几乎不单独使用。主流是激光雷达 + IMU 的紧耦合（LIO），视觉作为辅助。原因很实际：激光直接测距、不受光照影响、不存在尺度问题、弱纹理照样工作；而固态激光雷达（Livox）2020 年后价格降到千元级，消费级产品（扫地机器人、割草机器人）都用得起。

这条线的代表作都在本目录：20_LIO-SAM（2020，因子图框架）、21_FAST-LIO（2021，迭代卡尔曼）、22_FAST-LIO2（2022，ikd-Tree 增量地图，直接配准原始点）。FAST-LIO2 及其衍生（Faster-LIO、Point-LIO、iG-LIO）是 2026 年工业界最常见的里程计方案。再加上视觉做回环和上色就是 23_LVI-SAM、24_R3LIVE 和 FAST-LIVO2（2024）。

对你的意义。 这本书教的数学完全适用于 LIO------FAST-LIO 的迭代卡尔曼就是第 6 讲最小二乘的递推形式，它的状态在 SE(3)×ℝ³×... 的流形上，更新用的是第 4 讲的 ⊞；LIO-SAM 的因子图就是第 9、10 讲的图优化加上 IMU 预积分因子（05_Forster 预积分）。换句话说，视觉 SLAM 是学这套数学最好的入口（因为相机便宜、数据集多、能看见），但学完之后应该马上把它用到 LIO 上去，那才是工业上用的东西。你在 EMF 项目里做的 ESKF 和 IMU 融合，离 LIO 只差一个激光观测模型。

2.5 变了的：端侧部署成了核心能力

书的实践全部在 x86 笔记本上跑。2026 年 SLAM 跑在哪里？扫地机器人的 ARM 芯片、无人机的 Jetson、AR 眼镜的专用 SoC、人形机器人的 Orin。算法能不能在 10 W 功耗下实时跑成了选择方案的第一标准。这带来几个具体变化：

• 前端的学习型特征必须量化、必须跑在 NPU 上。SuperPoint INT8 在 Jetson Orin 上几毫秒一帧已是常态。

• 后端的 BA 要控制窗口大小和迭代次数，很多系统退回到了滤波（MSCKF 类，比如 OpenVINS）而不是优化，因为滤波计算量恒定。

• 多传感器时间同步（硬件触发、PTP）、标定（相机-IMU 外参、时间偏移）成了工程重点，一个没标好的时延能毁掉整个系统的精度。

对你的意义。 你的嵌入式背景在这里是优势而不是劣势。能读懂 SLAM 数学的人不少，能把它塞进 ARM + NPU 并保证实时和稳定的人很少。RK3566 上跑过 Linux 驱动、N6 上部署过 NPU 模型、调过 DMA 和中断优先级------这些经验正是把 SLAM 从论文变成产品的那一段。

2.6 变了的：SLAM 正在融入"具身智能"

2024 年以来，大模型驱动的机器人（具身智能）成为热点，SLAM 的角色随之变化。传统 SLAM 回答"我在哪、周围的几何是什么"；具身智能还要回答"周围有什么东西、它们能干什么、我该往哪走"。这推动了几个方向：

• 语义 SLAM / 开放词汇地图：用 CLIP、SAM 等视觉基础模型给地图上的点或高斯打语义标签，可以用自然语言查询地图（"厨房的冰箱在哪"）。ConceptGraphs、OpenScene 等是代表。

• 动态场景：传统 SLAM 假设世界静止，人、车、宠物都是要剔除的外点；现在的系统要显式地建模和跟踪动态物体。

• 端到端的空间智能：一些工作（如 Spatial-VLA、各种"world model"）试图让大模型直接学会空间推理，绕过显式 SLAM。它们目前精度远不如几何方法，但方向值得关注------SLAM 可能从"独立模块"变成"大模型的一个工具"。

**对你的意义。**短期内（3~5 年）几何 SLAM 不会被取代，因为精度、可靠性、可解释性都是硬需求；但 SLAM 工程师的工作内容会从"调一个 SLAM 系统"变成"把 SLAM 作为感知底座集成进一个更大的智能系统"，这要求你同时懂几何和学习两边。

2.7 一张表：这本书的内容在 2026 年的地位

书中内容	2026 年地位	建议
第 3~6 讲 数学基础	完全保值，所有方法的共同基础	精读，必须亲手推导
第 7 讲 ORB / 匹配	原理必须懂，具体算法已被学习型方法替代	理解几何（对极、PnP、ICP），了解 SuperPoint/LightGlue 接口
第 7 讲 PnP / ICP / 三角化	完全保值	精读，尤其是重投影误差雅可比
第 8 讲 光流 / 直接法	光流仍广泛使用，直接法是小众但重要的路线	理解光度误差和它的雅可比链
第 9 讲 BA / Schur / 鲁棒核	完全保值	精读
第 10 讲 滑窗 / 边缘化 / 位姿图	完全保值，LIO 和 VIO 都用	精读，特别注意 FEJ
第 11 讲 词袋回环	原理仍用，DBoW 正被学习型地点识别（NetVLAD 等）替代	理解思路，知道替代品
第 12 讲 TSDF / 八叉树	导航场景仍是主力；重建场景被 3DGS 取代	了解，重建方向直接学 3DGS
第 13 讲 工程框架	完全保值	认真读，多线程和生命周期是实际 bug 的主要来源
第 14 讲 开源方案	名单已过时	用下面的新名单替换

2026 年值得知道的开源系统（替换书第 14 讲的名单）：

• 视觉 / 视觉惯性：ORB-SLAM3（仍是特征法标杆）、VINS-Fusion、OpenVINS（滤波路线，端侧友好）、DSO/DM-VIO（直接法）

• 激光惯性：FAST-LIO2、Point-LIO、LIO-SAM、KISS-ICP（极简激光里程计）

• 多传感器融合：FAST-LIVO2、R3LIVE、LVI-SAM

• 稠密 / 神经建图：SplaTAM、MonoGS、Gaussian-SLAM、NICE-SLAM

• 学习型前端：SuperPoint、ALIKED、LightGlue、LoFTR、DUSt3R/MASt3R、VGGT

• 评估工具：evo（轨迹）、rpg_trajectory_evaluation

---

第三部分　读完这本书之后

3.1 本目录论文的推荐阅读顺序（修订版）

README_文献索引.md 里的五阶段路线写于读书之前。读完书之后，建议按下面的顺序，每篇都带着"它在书的哪一讲基础上做了什么"的问题去读：

1. 01_Cadena 综述------现在读它第 2~8 节会很有感觉，书里每个模块在综述里都有对应的"开放问题"。

2. 10_ORB-SLAM → 11_ORB-SLAM2------把书第 7、9、10、11 讲组装成一个系统是什么样。重点看三线程架构和关键帧策略。

3. 05_Forster 预积分------IMU 怎么进入优化框架。这是从"视觉 SLAM"走向"任何传感器 SLAM"的桥。你在 EMF 里积过 IMU，这篇会读得很顺。

4. 14_VINS-Mono------视觉 + IMU 紧耦合的标准做法，滑窗、边缘化、初始化一气呵成。

5. 12_ORB-SLAM3------视觉惯性 + 多地图，2021 年特征法的顶点。

6. 13_DSO------换一条路线看直接法怎么做。

7. 21_FAST-LIO → 22_FAST-LIO2------从视觉切到激光，从优化切到迭代卡尔曼。看它们如何用同一套流形数学解决不同传感器的问题。

8. 20_LIO-SAM → 23_LVI-SAM → 24_R3LIVE------因子图框架下的多传感器融合。

9. 02_Solà 李群、03_Barfoot 教材、04_Grisetti 图优化------这三篇是工具书，不必通读，遇到推导卡壳时查。

3.2 最值得亲手做的三件事

读十篇论文不如跑通一个系统。按投入产出比排序：

**第一，在自己的 Linux 机器上编译并跑通 ORB-SLAM3 + EuRoC 数据集，用 evo 算出 ATE。**这一步把书的全部内容变成一个可以观察、可以改参数、可以插日志的真实对象。详见下一册《Linux 上手实操指南》。

**第二，把书第 13 讲的精简 SLAM 加上回环检测。**书留了这个作业。做完它，你就完整地实现过一次"前端 + 后端 + 回环"，比任何面试题都有说服力。

**第三，把 FAST-LIO2 跑在一个带 IMU 的激光雷达上（或它的数据集），然后读懂它的 IMU_Processing.hpp 和 use-ikfom.hpp。**这是从书本走向工业的一步。你会发现它的状态更新代码就是 Solà 论文的 ⊞，它的卡尔曼增益公式就是第 6 讲最小二乘的另一种写法。

3.3 结语

这本书的书名叫"十四讲"，但它真正教的是一件事：**如何把一个物理世界的几何问题，写成一个概率问题，再写成一个最小二乘问题，然后在流形上迭代求解。**相机、特征、词袋、八叉树，都是这件事的具体实例。学会了这件事，换成激光雷达、换成 IMU、换成毫米波雷达、换成电磁线圈，流程是一样的------你在 EMF 项目里做过的"把线圈耦合写成观测方程、用 ESKF 求位姿"，和这本书是同一件事。

2026 年的 SLAM 和 2019 年相比，前端换了神经网络，地图换了高斯球，主力传感器换了激光雷达，运行平台换成了 ARM + NPU，上面还多了一层大模型。但解增量方程的那一行代码没有变。把不变的学透，把变的保持关注，这是对待这个领域最务实的态度。