3. 视觉SLAM数学基础
视觉SLAM的本质，是在连续图像观测中估计相机位姿与三维地图点。要让这件事在数学上成立，必须先把“相机在哪里、朝哪看、像素如何成像、两帧之间如何约束”这一整条链路描述清楚。本节把这条链路拆成六个相互衔接的知识块：坐标系变换、旋转表示、李群李代数、相机模型、畸变模型、对极几何与三角测量。它们不是孤立公式，而是一条从世界坐标到像素坐标、再从像素反推三维结构的完整通路。
一、坐标系变换：SLAM的“空间语法”
SLAM中常见的坐标系有世界系 W、机体/IMU系 B、相机系 C、图像系 I。一个空间点 P 在不同系下有不同坐标，变换由旋转 R 与平移 t 组成：
P_C = R_CW · P_W + t_CW
T_CW = [ R_CW t_CW ]
[ 0 1 ]其中 T_CW 是4×4齐次变换矩阵，表示“把世界系点变换到相机系”。注意下标方向：T_CW 的逆 T_WC 才是相机在世界系中的位姿。实际工程中，外参标定给出的常是 T_BC（相机在机体系下），而VIO估计的是 T_WB，因此相机位姿需链式相乘：T_WC = T_WB · T_BC。方向写反是初学者最常见的错误之一。
二、旋转表示：四种语言与各自代价
旋转是SLAM中更新最频繁的量，常见表示有旋转矩阵、欧拉角、轴角、四元数。它们各有适用场景：
| 表示 | 参数个数 | 优点 | 缺点/风险 |
|---|---|---|---|
| 旋转矩阵 R | 9（约束6） | 无奇异、可直接复合 | 冗余，优化时需正交约束 |
| 欧拉角 | 3 | 直观、易读 | 万向锁、插值不自然 |
| 轴角 | 3+1 | 几何意义清晰 | 0角附近不稳定 |
| 四元数 q | 4（约束1） | 紧凑、插值平滑 | 双覆盖 q 与 -q 同旋转 |
在优化中，旋转矩阵和四元数都带约束，不能直接做无约束最小二乘。这正是李群李代数要解决的问题。
三、李群李代数：让旋转可微、可优化
旋转矩阵构成特殊正交群 SO(3)，刚体变换构成 SE(3)。它们是李群，局部可用李代数 so(3)、se(3) 的向量 φ、ξ 近似。指数映射把李代数送到李群：
R = exp(φ^) ≈ I + φ^ // 小量近似
T = exp(ξ^) // ξ = [ρ, φ]优化时，我们不再直接更新 R，而是求增量 δφ，用左乘或右乘更新：R ← R · exp(δφ^)。这样每次迭代都在切空间做无约束更新，再映射回流形。雅可比矩阵（如 ∂(Rp)/∂δφ = -R p^）是后端优化的核心，也是g2o、Ceres、GTSAM等库自动求导或解析求导的基础。
四、相机模型与畸变模型：从三维点到像素
针孔相机模型把相机系点 P_C = [X, Y, Z]^T 投影到归一化平面，再经内参 K 得到像素：
u = fx · X/Z + cx
v = fy · Y/Z + cy
K = [ fx 0 cx ]
[ 0 fy cy ]
[ 0 0 1 ]真实镜头存在畸变。径向畸变用 k1, k2, k3 描述，切向畸变用 p1, p2 描述：
x_dist = x(1 + k1 r² + k2 r⁴ + k3 r⁶) + 2p1xy + p2(r² + 2x²)
y_dist = y(1 + k1 r² + k2 r⁴ + k3 r⁶) + p1(r² + 2y²) + 2p2xy去畸变有两种策略：一是先对整幅图像去畸变再提特征，二是保留畸变模型、在投影时直接带入畸变。后者精度更高，但雅可比更复杂。鱼眼相机还需使用KB模型或等距模型，不能直接套用上述多项式。
五、对极几何与三角测量：两帧如何约束三维
同一空间点在两帧中的像素 x1、x2 满足对极约束：
x2^T · F · x1 = 0 // F 为基础矩阵
x2^T · E · x1 = 0 // E = t^ R，本质矩阵
E = K2^T · F · K1对极约束把匹配点限制在极线上，把二维搜索降为一维。已知相机内参和匹配点，可用八点法求 E，再分解得到 R, t。但单目尺度不可观，t 只有方向没有长度。三角测量则用两帧射线相交恢复深度：
s1·x1 = s2·R·x2 + t
// 最小二乘求解 s1, s2，得到三维点三角测量的精度取决于基线长度和视差：基线太短、视差太小，深度不确定性急剧放大。这也是单目初始化必须等待足够平移的原因。
六、案例与场景：单目初始化中的数学链路
以单目VIO初始化为例：系统先提取两帧特征并匹配，用对极几何求 E 并分解出 R, t 方向；随后三角测量得到一批三维点；再用这些点做PnP估计更精确的位姿；最后把尺度与IMU预积分对齐。整条链路中，坐标系变换保证外参正确，李代数保证位姿可优化，相机与畸变模型保证投影准确，对极几何与三角测量保证结构可恢复。任何一环出错，都会表现为“轨迹漂移”或“地图点飞散”。
T_CW 与 T_WC 混用；用欧拉角直接做优化更新；忽略畸变导致大视场边缘匹配误差；基线过短时强行三角测量，深度噪声被放大数倍。七、复盘清单
- 变换方向：每次写
T_AB时先问“A到B还是B到A”。 - 旋转更新：优化中优先使用李代数扰动，避免直接更新旋转矩阵。
- 投影链路：确认内参、畸变、外参的标定版本一致。
- 对极约束：匹配点需先去畸变，否则
F估计有偏。 - 三角测量：检查视差角，低于阈值时不要纳入地图点。
掌握这六个知识块，就具备了阅读ORB-SLAM、VINS-Mono、OpenVINS等系统代码的数学基础。后续的视觉里程计、后端优化、回环检测，都是在这条链路上做工程化扩展。