第1章：SLAM算法核心模块全景解析
大家好，我是老张。做嵌入式SLAM这些年，我最大的感触就是——算法再漂亮，跑不动也是白搭。今天咱们聊聊SLAM的三大核心模块：前端视觉里程计、后端优化、回环检测与地图构建。这三个模块说白了就是SLAM的骨架，搞懂了它们，你就能理解整个系统是怎么转起来的。
核心观点：SLAM不是单一算法，而是一个多模块协同的系统。每个模块都有其独特的计算特性和优化空间。
1.1 前端视觉里程计：SLAM的"眼睛"
前端视觉里程计，说白了就是通过连续图像帧来估计相机运动。我刚开始做SLAM时，总觉得这步就是算个位姿嘛，能有多难？结果第一次在嵌入式平台上跑ORB-SLAM，帧率直接掉到个位数...嗯，从那以后我再也不敢小看前端优化了。
前端主要分两派：特征点法和直接法。咱们一个一个说。
特征点法
特征点法是目前最成熟的方法。它的流程很清晰：
- 特征提取：从图像中提取关键点和描述子（ORB、SIFT、SURF等）
- 特征匹配：通过描述子距离找到帧间对应关系
- 位姿估计：用PnP或ICP算法求解相机运动
我个人习惯用ORB特征，原因很简单——它够快。在ARM Cortex-A72上，640×480的图像提取ORB特征大概需要15-20ms，而SIFT要100ms以上。你想想看，在嵌入式平台上，这差距就是能不能实时运行的分水岭。
我的经验：在嵌入式平台上，特征点数量要动态调整。场景纹理丰富时用500个点，纹理稀疏时用1000个。我曾经在仓库场景中固定用1000个点，结果CPU占用率飙到90%，后来改成动态调整，直接降到40%。
直接法
直接法不走特征提取的路子，它直接利用像素灰度信息来估计位姿。代表算法有LSD-SLAM、DSO等。
直接法的优势在于：
- 不需要特征提取，计算量更小
- 在纹理稀疏场景表现更好
- 可以构建半稠密/稠密地图
但直接法也有坑。我记得有一次在光照变化剧烈的环境下跑DSO，结果位姿估计直接发散。后来分析发现，直接法对光照变化太敏感了。所以现在我做项目，室内场景用直接法，室外场景用特征点法。
注意：直接法在嵌入式平台上要特别注意图像金字塔的层数。层数太多，内存带宽扛不住；层数太少，大运动估计不准。我一般用3-4层，每层降采样2倍。
1.2 后端优化：SLAM的"大脑"
前端给的位姿估计，说白了就是"近视眼"——只看相邻帧，误差会不断累积。后端优化的作用就是把这些误差"抹平"。
后端优化主要有两条路：图优化和滤波器。
图优化
图优化是目前的主流方法。它把相机位姿和路标点看作图的节点，观测关系看作边，然后通过最小化误差来优化所有节点。
核心公式很简单：
// 图优化的目标函数
min Σ ||z_ij - h(x_i, x_j)||²_Σ
其中：
- z_ij：观测值
- h(x_i, x_j)：预测值
- Σ：信息矩阵（协方差的逆）常用的图优化库有g2o和GTSAM。我个人更倾向g2o，因为它的接口更直观，调试起来方便。不过GTSAM的iSAM2增量式求解在嵌入式平台上表现更好——它只更新受影响的部分，而不是每次都全局优化。
关键点：在嵌入式平台上，图优化的瓶颈在于矩阵求解。我建议用稀疏Cholesky分解，配合Schur补技巧，能把计算量降低一个数量级。
滤波器方法
滤波器方法（EKF、UKF、粒子滤波）是早期的方案，现在用得少了。但在计算资源极度受限的场景下，它仍有优势。
EKF的流程：
- 预测：根据运动模型预测当前状态
- 更新：用观测值修正预测结果
- 迭代：重复上述两步
我记得在STM32F4上做过一个EKF-SLAM，只用了不到100KB内存就能跑起来。虽然精度不如图优化，但胜在轻量。如果你在做超低功耗的SLAM，EKF还是值得考虑的。
| 方法 | 精度 | 计算量 | 内存占用 | 适用场景 |
|---|---|---|---|---|
| 图优化（g2o） | 高 | 高 | 高 | 高性能嵌入式平台 |
| 图优化（iSAM2） | 高 | 中 | 中 | 中等性能平台 |
| EKF | 中 | 低 | 低 | 超低功耗平台 |
| 粒子滤波 | 中高 | 中 | 高 | 非线性强的场景 |
1.3 回环检测与地图构建
回环检测解决的是"漂移"问题。你想想看，SLAM跑久了，位姿误差会越积越多，地图也会越来越歪。回环检测就是让机器人"认路"，发现曾经来过的地方，然后修正全局位姿。
回环检测的核心技术
目前最成熟的方法是词袋模型（Bag of Words）。它的思路是：
- 离线训练：从大量图像中提取特征，聚类成"视觉单词"
- 在线检测：将当前图像转为词袋向量，与历史帧匹配
- 几何验证：用PnP或ICP验证匹配是否正确
DBoW2和DBoW3是常用的词袋库。我建议用DBoW3，它支持增量式词典更新，在嵌入式平台上更灵活。
避坑指南：我曾经在回环检测中吃过亏——词袋匹配的阈值设得太低，导致大量误匹配。后来我加了一步几何验证，用RANSAC+PnP过滤掉错误匹配，回环准确率从70%提升到了95%以上。
地图构建
地图构建分三种：
- 稀疏地图：只保存路标点，内存占用小，适合定位
- 半稠密地图：保存有梯度的像素，适合导航
- 稠密地图：保存所有像素，适合避障和交互
在嵌入式平台上，我一般用稀疏地图做定位，半稠密地图做导航。稠密地图？除非你有GPU加速，否则别碰——一个640×480的深度图就要1.2MB，跑几分钟内存就爆了。
我的建议：地图构建要分阶段进行。前端实时运行时只维护稀疏地图，回环检测触发后再更新半稠密地图。这样既能保证实时性，又能保证地图质量。
1.4 三大模块的协同工作
这三个模块不是孤立的，它们之间有紧密的数据流：
- 前端输出帧间位姿和路标点
- 后端接收这些数据，进行局部优化
- 回环检测发现闭环后，触发全局优化
- 地图构建根据优化结果更新地图
在嵌入式平台上，这个流程要特别注意时序。我一般用多线程架构：前端跑在实时线程（30fps），后端跑在低优先级线程（5-10fps），回环检测跑在后台线程（1-2fps）。这样即使后端卡住了，前端也不会掉帧。
好了，这一章的内容就到这里。下一章咱们会深入前端视觉里程计，聊聊特征提取在嵌入式平台上的具体优化技巧。
公众号:蓝海资料掘金营，微信deep3321