2. 数学基础

视觉惯性里程计（VIO）的核心任务，是在三维空间中持续估计传感器（相机与IMU）的运动轨迹与姿态。无论是特征点三角化、IMU预积分，还是滑动窗口优化，所有计算都建立在严谨的数学表达之上。本章将系统梳理VIO中最基础的数学工具：从坐标系定义与刚体变换出发，对比旋转矩阵与四元数的表达差异，最后引入李群与李代数（SO(3)、SE(3)）及其在优化中的核心作用。这些概念不仅是公式推导的基石，更直接决定了工程实现中数值稳定性与计算效率。

上图展示了本章的知识框架：坐标系定义是描述一切运动的前提；旋转矩阵与四元数是两种最常用的姿态表达方式；而李群与李代数则连接了“状态表达”与“优化求解”之间的鸿沟。下面我们逐一深入剖析。

2.1 坐标系定义与刚体变换

核心原理：在VIO系统中，我们通常定义三个主要坐标系：世界坐标系（W，通常与IMU初始位置对齐或定义为重力方向为Z轴）、相机坐标系（C，光心为原点，Z轴指向相机前方）、IMU坐标系（I，加速度计与陀螺仪的敏感轴方向）。刚体变换描述了一个坐标系相对于另一个坐标系的旋转与平移，数学上表示为 T = [R | t]，其中 R ∈ SO(3) 是旋转矩阵，t ∈ R³ 是平移向量。齐次坐标形式为 T ∈ SE(3)，即 4×4 矩阵。

操作流程：在实际工程中，坐标变换遵循“链式法则”。例如，将相机观测到的3D点 p_C 转换到世界系：

p_W = R_WC * p_C + t_WC
     = T_WC * p_C_homogeneous

其中 T_WC 表示从相机系到世界系的变换。VIO中常见的变换链为：T_WI = T_WC * T_CI，即通过外参 T_CI（相机-IMU外参）将IMU位姿转换到相机位姿。

2.2 旋转矩阵与四元数

核心原理：旋转矩阵 R 是正交矩阵（RᵀR = I，det(R)=1），拥有9个参数但只有3个自由度，因此存在冗余。四元数 q = [w, x, y, z] 是单位四元数（w²+x²+y²+z²=1），用4个参数表达3个自由度，避免了欧拉角的万向锁问题，且插值方便。两者可以互相转换：

// 四元数转旋转矩阵（Eigen）
Eigen::Matrix3d R = q.toRotationMatrix();
// 旋转矩阵转四元数
Eigen::Quaterniond q(R);

判断方法：在VIO中，选择哪种表达方式取决于使用场景。优化变量通常使用四元数（因为归一化约束容易处理），而坐标变换计算时使用旋转矩阵（因为矩阵乘法效率高）。IMU预积分中，旋转增量使用四元数乘法：Δq = q_i ⊗ q_j。

2.3 李群与李代数：SO(3) 与 SE(3)

核心原理：李群是连续光滑的群，SO(3) 是三维旋转群，SE(3) 是三维刚体运动群。李代数是对应李群在单位元处的切空间，SO(3) 对应 so(3)（三维反对称矩阵），SE(3) 对应 se(3)（六维向量，前三维平移，后三维旋转）。核心关系通过指数映射和对数映射连接：

R = Exp(φ) = I + sin(θ) * φ_hat + (1-cos(θ)) * φ_hat²   // Rodrigues公式
φ = Log(R)  // 旋转向量

其中 φ 是旋转向量，θ = ||φ||，φ_hat 是 φ 的反对称矩阵。

操作流程：在VIO的优化问题中，我们通常将位姿 T 视为李群元素，但在求导时使用李代数。具体步骤为：

1. 定义残差 r = z - h(T)，其中 z 是观测值（如特征点像素坐标），h 是预测模型。
2. 对 T 施加左扰动 δξ（李代数增量），计算雅可比 J = ∂r/∂δξ。
3. 求解增量方程 H δξ = b，更新 T ← T * Exp(δξ)。

案例拆解：以视觉重投影误差为例。设相机位姿 T_WC，3D点 p_W，观测像素 z。残差为 r = z - π(T_WC * p_W)。对 T_WC 求导时，使用左扰动模型：

J = ∂r/∂δξ = ∂r/∂p_C * ∂p_C/∂δξ
其中 ∂p_C/∂δξ = [I, -p_C_hat]  // 平移部分和旋转部分

这种求导方式避免了旋转矩阵的复杂链式法则，且雅可比形式简洁，便于在Ceres或g2o中实现。

2.4 本章总结与常见误区

核心要点回顾：坐标系定义是VIO的“世界语言”，刚体变换 T ∈ SE(3) 是连接所有坐标系的桥梁。旋转矩阵适合直观理解与坐标变换，四元数适合状态存储与插值，李代数适合优化求导。三者之间的转换关系必须熟练掌握。

常见误区清单：

• 误区1：混淆旋转矩阵的转置与逆。对于旋转矩阵，R⁻¹ = Rᵀ，但平移向量不能直接转置，必须使用 t' = -Rᵀ * t。
• 误区2：四元数乘法顺序。四元数乘法 q1 ⊗ q2 表示先应用 q2 再应用 q1，与旋转矩阵乘法顺序相反，容易出错。
• 误区3：忽略李代数的局部有效性。指数映射仅在原点附近是局部同胚，当旋转角度接近 π 时，对数映射可能产生奇异，需做特殊处理。
• 误区4：在优化中直接对四元数四个分量求导，而不施加单位约束。这会导致优化结果不再是合法旋转，必须使用扰动模型。