一、SLAM概述与数学基础
大家好，欢迎来到视觉SLAM的第一章。
说实话，每次带新人入门SLAM，我最怕的就是他们一上来就啃论文、调代码，结果连「我们在解决什么问题」都没搞清楚。所以这一章，咱们先把地基打牢。
1.1 SLAM问题定义
SLAM，全称是 Simultaneous Localization and Mapping，中文叫「同时定位与地图构建」。说白了就是：让一个机器人在未知环境中，一边走路，一边给自己定位，一边画地图。
你想想看，这其实是个「鸡生蛋蛋生鸡」的问题：
- 要知道自己在哪里，得先有地图
- 要建地图，得先知道自己在哪
我刚开始接触这个领域时，觉得这根本就是个死循环。后来才明白，SLAM的精髓就在于——用概率和优化，把这个循环解耦。
核心公式（贝叶斯视角）：
SLAM 本质上是在求解后验概率：
P(x, m | z, u)其中 x 是机器人位姿，m 是地图，z 是观测，u 是控制输入。
嗯，这里要注意：我们不是一次性求解所有状态，而是用滤波器或图优化的方式，逐步逼近最优解。
1.2 坐标系与刚体运动
做SLAM，坐标系是绕不开的坎。我个人习惯把坐标系分成三类：
| 坐标系 | 符号 | 说明 |
|---|---|---|
| 世界坐标系 | W | 固定不动，所有东西的绝对参考 |
| 相机坐标系 | C | 以相机光心为原点，Z轴朝前 |
| 像素坐标系 | uv | 图像上的行列坐标 |
刚体运动，说白了就是「物体在空间里又转又移，但形状不变」。数学上用一个旋转矩阵 R 和一个平移向量 t 来描述：
P' = R * P + t我在项目中遇到过一个问题：有人把旋转矩阵写成了转置，结果地图全反了。嗯，这种坑踩一次就记住了。
1.3 四元数与李代数
说到旋转表示，很多人第一反应是欧拉角。但我建议你尽快忘掉它——欧拉角有万向锁问题，而且在插值时表现很差。
我个人最常用的是四元数和李代数。
四元数
四元数可以理解为「带约束的复数扩展」：
q = w + xi + yj + zk
约束：w² + x² + y² + z² = 1为什么用四元数？因为它没有奇异性，而且计算效率高。我曾经在嵌入式平台上做SLAM，用欧拉角算一次旋转要花3微秒，换成四元数只要1.2微秒——在实时系统里，这差距就是能不能跑30帧的关键。
李代数
李代数（so(3) 和 se(3)）是SLAM优化的核心工具。它把旋转矩阵的乘法，变成了向量空间的加法：
so(3) → 三维向量 φ
se(3) → 六维向量 ξ为什么要这么折腾？因为优化问题在向量空间里好解啊！你想想看，在流形上做梯度下降多麻烦，但在李代数上，直接套用最小二乘法就行。
避坑指南：
我曾经在代码里直接用 Eigen 的 AngleAxis 做优化，结果迭代到第5步就发散。后来换成 Sophus 的李代数实现，收敛稳定多了。记住：优化一定要在切空间里做。
1.4 三维空间刚体运动
把前面说的东西串起来，三维空间刚体运动可以表示为：
T = [R t]
[0 1]这个 4×4 的变换矩阵 T，属于 SE(3) 群。它把世界坐标系下的点，映射到相机坐标系下。
实际应用中，我们经常需要做以下操作：
- 坐标变换：把激光雷达的点云转到相机坐标系
- 位姿插值：在两个关键帧之间平滑过渡
- 误差计算：比较估计位姿和真实位姿的差异
我记得有一次做多传感器融合，IMU和相机的坐标系没对齐，结果轨迹直接飞了。后来花了整整两天才排查出来——原来是旋转顺序搞反了。所以我现在写代码，一定会把坐标系变换写成函数，并且加上单元测试。
重要提醒：
刚体运动中的旋转矩阵 R 必须满足：
- RᵀR = I（正交性）
- det(R) = 1（右手系）
如果数值误差导致 R 不满足这些条件，一定要做正交化处理。我见过有人直接拿带误差的 R 去算，结果地图越跑越歪。
知识体系总览
下面这张图，是我自己总结的本章知识结构：
好了，这一章的内容就到这里。数学基础虽然枯燥，但它是后面所有算法的根基。我建议你把四元数和李代数的推导亲手算一遍，代码也敲一遍——相信我，这会让你少走很多弯路。