一、SLAM基础回顾：经典视觉SLAM框架
做SLAM这么多年，我经常被问到：「视觉SLAM到底是怎么工作的？」其实说白了，它就是一个让机器人或设备在未知环境中同时定位自身并构建地图的过程。今天咱们就从最经典的框架讲起，把那些基础概念掰开揉碎了说清楚。
1.1 经典视觉SLAM框架的四个模块
我记得刚入行时，导师给我画了一张图，就是经典的SLAM框架。这么多年过去了，这个框架依然管用。它主要由四个模块组成：
视觉SLAM框架核心流程
嗯，这里要注意，这四个模块不是独立工作的。前端给后端喂数据，后端做优化，回环检测发现问题后反馈给后端修正，最后建图模块把结果呈现出来。我在项目中遇到过不少同学把回环检测当成可选项，结果跑着跑着轨迹就飘到天边去了。
1.2 前端：视觉里程计
前端说白了就是「看路」的。它负责从连续图像中估计相机的运动。这里有两种主流方法：
特征点法
我个人习惯用ORB特征，因为它又快又稳。特征点法的核心步骤是：
- 特征提取：从图像中找出角点、斑点等显著区域
- 特征匹配：在相邻帧之间找到对应的特征点
- 运动估计：利用匹配点对计算相机位姿变化
我的经验：特征点法对光照变化比较鲁棒，但在纹理稀疏的场景（比如白墙）会直接「瞎掉」。我曾经在一条纯白色的走廊里跑ORB-SLAM，结果前端直接罢工了。
直接法
直接法不走特征提取的路子，它直接利用像素灰度信息来估计运动。LSD-SLAM和DSO就是典型代表。
- 优点：在纹理稀疏场景表现更好，可以构建半稠密地图
- 缺点：对光照变化敏感，容易受图像噪声影响
你想想看，直接法就像是用整张图片的「灰度场」来做匹配，而特征点法只盯着几个「关键点」看。各有各的适用场景。
1.3 后端：非线性优化
后端是SLAM的「大脑」。它负责把前端估计的位姿和地图点进行全局优化。这里我重点说一下图优化。
图优化把SLAM问题建模成一个图：节点是相机位姿和路标点，边是观测约束。然后通过最小化误差函数来优化所有变量。
// 一个简单的图优化示例（g2o风格）
// 顶点：相机位姿
VertexSE3* pose = new VertexSE3();
pose->setId(0);
pose->setEstimate(Eigen::Isometry3d::Identity());
// 边：观测约束
EdgeProjectXYZ2UV* edge = new EdgeProjectXYZ2UV();
edge->setVertex(0, pose);
edge->setMeasurement(observation);
edge->setInformation(Eigen::Matrix2d::Identity());
// 优化
optimizer.initializeOptimization();
optimizer.optimize(10);避坑指南：我曾经在优化时忘记设置信息矩阵（Information Matrix），结果优化结果完全不对。信息矩阵代表了观测的不确定性，千万别省这一步。
1.4 回环检测
回环检测解决的是「漂移」问题。SLAM跑久了，误差会累积，轨迹会越来越偏。回环检测就是让系统「认出」曾经来过的地方，然后修正累积误差。
常用的方法是词袋模型（Bag of Words）。它把图像描述成视觉词汇的统计直方图，通过比较直方图来判断两幅图像的相似度。
| 方法 | 精度 | 速度 | 适用场景 |
|---|---|---|---|
| 词袋模型 | 中等 | 快 | 大规模场景 |
| 深度学习 | 高 | 慢 | 复杂环境 |
| 几何验证 | 高 | 中等 | 小范围场景 |
1.5 建图
建图模块把优化后的结果变成可用的地图。根据应用需求不同，地图形式也不同：
- 稀疏地图：只保存特征点，适合定位用
- 半稠密地图：保存边缘或梯度明显的区域，适合导航
- 稠密地图：保存所有像素，适合避障和交互
1.6 相机模型与坐标系
做SLAM必须搞懂相机模型。最常用的是针孔相机模型，它把三维世界投影到二维图像上。
四个坐标系要分清楚：
- 世界坐标系：固定不变，描述物体在真实世界的位置
- 相机坐标系：以相机光心为原点
- 图像坐标系：在成像平面上，单位是毫米
- 像素坐标系：在图像上，单位是像素
核心公式：从世界坐标到像素坐标的投影过程
// 世界点 P_w 到像素点 p 的投影
// 1. 世界 -> 相机
P_c = R * P_w + t
// 2. 相机 -> 归一化平面
P_n = [P_c.x / P_c.z, P_c.y / P_c.z]
// 3. 归一化 -> 像素（考虑畸变）
p.x = f_x * P_n.x + c_x
p.y = f_y * P_n.y + c_y1.7 李群与李代数基础
说到SLAM优化，就绕不开李群和李代数。为什么需要它们？因为旋转矩阵有约束条件（正交且行列式为1），直接优化很麻烦。
李代数把旋转矩阵映射到三维向量空间，让优化变得简单。常用的有：
- SO(3)：三维旋转群，对应李代数 so(3)
- SE(3)：三维刚体变换群，对应李代数 se(3)
指数映射和对数映射是连接李群和李代数的桥梁：
// 李代数 -> 李群（指数映射）
Eigen::Matrix3d R = Eigen::AngleAxisd(theta, axis).toRotationMatrix();
// 李群 -> 李代数（对数映射）
Eigen::AngleAxisd angle_axis(R);
double theta = angle_axis.angle();
Eigen::Vector3d axis = angle_axis.axis();我的建议：刚开始学李群李代数时，别被数学公式吓到。你只需要记住：李代数让旋转变成了「可加」的向量，这样就能用常规的优化方法了。我在做BA优化时，就是用李代数来表示位姿增量，效果非常好。
好了，以上就是SLAM基础的核心内容。这些概念是后续学习语义SLAM的基石，尤其是后端优化和坐标系变换，在融合语义信息时会频繁用到。