2. 数学基础
VINS（Visual-Inertial Navigation System）的核心任务，是持续估计载体在三维空间中的位置与姿态。无论是视觉前端提取的特征点三角化，还是IMU预积分与后端非线性优化，所有计算都建立在“如何精确描述刚体运动”这一数学地基之上。本章将系统梳理三维空间刚体运动的四种主流表达方式——旋转矩阵、变换矩阵、四元数与李群/李代数，并重点剖析它们在VINS前端、后端与状态更新中的适用场景与转换关系。
核心原理：四种表达方式的数学本质与内在联系
三维空间中的刚体运动可以分解为旋转和平移两部分。旋转矩阵 R 属于特殊正交群 SO(3)，满足 RTR = I 且 det(R) = +1，其9个元素只有3个自由度，因此存在6个隐含约束。变换矩阵 T 属于特殊欧氏群 SE(3)，将旋转与平移统一为4×4齐次矩阵，使得连续变换可以简单地通过矩阵乘法复合。四元数 q 是四维单位向量，用3个虚部+1个实部表示旋转，避免了欧拉角的万向锁问题，且插值平滑。李群与李代数则提供了在流形上进行无约束优化的数学工具——旋转矩阵的导数不再是旋转矩阵，而李代数 so(3) 是三维向量空间，其指数映射可以精确地将旋转向量转换为旋转矩阵。
这四种表达方式并非孤立存在，它们之间可以相互转换。旋转矩阵与四元数之间的转换公式在VINS初始化、相机-IMU外参标定中频繁使用；李代数与旋转矩阵的指数/对数映射则是后端非线性优化的核心操作。理解这些转换关系，是后续推导视觉重投影误差雅可比矩阵和IMU预积分误差传播的前提。
判断方法：如何根据场景选择合适的运动表达
在VINS系统中，不同模块对运动表达的需求差异显著，选择不当会导致数值不稳定或计算效率低下。下表总结了各场景下的推荐选择与理由：
| 应用场景 | 推荐表达 | 核心原因 |
|---|---|---|
| 视觉特征三角化 | 变换矩阵 T | 齐次坐标便于投影与反投影运算 |
| IMU姿态积分 | 四元数 q | 避免万向锁，插值平滑，存储紧凑 |
| 后端图优化 | 李代数 ξ | 无约束优化，避免正交性约束 |
| 相机-IMU外参标定 | 旋转矩阵 R | 便于构建旋转约束方程 |
| 闭环检测位姿计算 | 四元数+平移向量 | 便于相似变换计算与回环校正 |
| 可视化与调试 | 欧拉角（仅显示） | 直观，但注意奇异性 |
选择的基本原则是：计算与存储优先用四元数，优化与求导优先用李代数，坐标变换与投影优先用变换矩阵。在VINS代码实现中，状态向量通常以四元数+位置+速度+零偏的形式存储，而在优化求解时则切换到李代数扰动模型。
案例拆解：VINS初始化中的旋转矩阵与四元数转换
以VINS-Mono的初始化阶段为例，系统需要估计相机与IMU之间的旋转外参 Rcb。该过程首先利用视觉SFM恢复出相机的旋转四元数序列 qck，同时通过IMU预积分得到相邻帧间的旋转增量 Δqbk+1,bk。根据刚体运动约束，两者应满足：
q_cb ⊗ q_ck = q_bk ⊗ q_cb其中 ⊗ 表示四元数乘法。将上式改写为线性方程 A·q_cb = 0，通过SVD分解即可求解外参四元数。在实际代码中，需要将四元数转换为旋转矩阵形式构建A矩阵，求解后再转回四元数。这一过程涉及四元数乘法、旋转矩阵与四元数互转、SVD分解三个关键步骤。若外参估计不准，后续的视觉-惯性对齐将产生系统性偏差，导致初始化失败或轨迹漂移。
该案例的工程启示在于：四元数适合作为状态存储格式，但在构建线性约束时往往需要转换为旋转矩阵。VINS代码中大量使用Eigen库的Quaterniond和Matrix3d类型，正是为了在两种表达间高效切换。
常见误区与总结复盘
误区一：混淆旋转矩阵与变换矩阵的维度。旋转矩阵只能表达姿态，不能表达位置；变换矩阵是4×4齐次矩阵，包含旋转与平移。在VINS中，若将旋转矩阵误当作变换矩阵使用，会导致平移分量丢失，产生严重的定位错误。
误区二：忽视四元数的单位约束。四元数必须满足模长为1。在优化过程中若直接对四元数分量求导，会破坏单位约束，导致数值发散。正确做法是使用李代数扰动模型，在切空间上优化。
误区三：欧拉角在VINS中的不当使用。欧拉角存在万向锁问题，且不同旋转顺序对应不同结果。VINS内部严禁使用欧拉角进行状态估计，仅在可视化调试时作为辅助显示。
经验总结：在VINS开发中，建议统一使用四元数作为状态存储格式，使用李代数进行优化求导，使用变换矩阵进行坐标变换。每次进行表达方式转换时，务必检查转换后的数值是否满足约束条件（如旋转矩阵的正交性、四元数的单位性）。在调试阶段，可以打印旋转矩阵的行列式值或四元数的模长，快速定位数值异常。
李群与李代数的核心价值在于将带约束的旋转矩阵优化问题转化为无约束的向量空间优化问题。so(3) 中的每个元素是一个三维旋转向量，其指数映射 exp(φ^) 生成对应的旋转矩阵；反之，对数映射 Log(R) 将旋转矩阵映射回旋转向量。在VINS后端优化中，残差对位姿的雅可比矩阵正是通过李代数扰动模型推导的，这避免了在流形上直接求导的复杂性。理解这一数学工具，是掌握VINS非线性优化精髓的关键一步。