FAST-LIVO2 源码精读（四）：SO(3) 流形、李代数与右扰动

FAST-LIVO2 源码精读（四）：SO(3) 流形、李代数与右扰动

本文是「FAST-LIVO2 激光-惯性-视觉里程计源码精读」专栏第四篇，也是数学基础篇章（第 4–7 篇）的开篇。前三篇完成了系统认知与实际运行；从本篇起深入数学，为后续 ESIKF 推导和源码逐行精读铺垫必要的工具。本篇聚焦一个根本问题：旋转为什么不能直接相加，以及 FAST-LIVO2 的 operator+ 是如何在代码层面解决这个问题的。

一、引子：一个隐蔽的错误——旋转角直接相加

考虑一个简单的场景：载体绕 Z 轴旋转了 45°，再绕 X 轴旋转了 90°；另一次操作，先绕 X 轴旋转 90°，再绕 Z 轴旋转 45°。两次旋转的角度之和完全相同，但最终姿态截然不同——三维旋转不满足交换律。

这个事实广为人知，但其对状态估计的影响往往被低估。卡尔曼滤波器的核心操作是"加"：将预测状态与观测残差的线性修正量相加，得到更新后的状态。 若直接将角度增量加到旋转的某种参数化表示上，则必须小心：哪些参数化允许直接加法，哪些不允许。

旋转矩阵 $R\in {\mathbb{R}}^{3\times 3}$ 满足两个约束：$R^{T}R=I$ 与 $\det (R)=1$。这九个元素中只有三个自由度，其余六个由约束固定。若将两个旋转矩阵直接相加（$R_1+R_2$），结果几乎必然不再满足正交约束，从而不再是一个有效旋转——旋转矩阵的集合在加法下不封闭。

用欧拉角也无法规避问题：三个角度 $(\varphi ,\theta ,\psi )$ 的加法在奇异点（万向锁）附近失去物理意义，且顺序依赖性使其在滤波器中难以稳定使用。

四元数提供了相对干净的参数化（乘法封闭），但将四元数增量与绝对四元数做加法后需重新归一化，破坏了线性代数的操作一致性。

正确的解决方案来自微分几何：将旋转矩阵的集合视为一个微分流形——李群 SO(3)，并借助与之对应的李代数 so(3) 在切空间中完成线性运算，再通过指数映射将结果送回流形。这套框架正是 FAST-LIVO2 operator+ 的数学基础，也是 ESIKF 在非欧空间中工作的前提。

二、最小必要集：李群、李代数与两个映射

完整的李群理论涉及抽象代数与微分几何的深层内容。以下仅提炼 FAST-LIVO2 实际使用的核心概念，以够用为原则。

2.1 李群 SO(3)

特殊正交群（Special Orthogonal Group）SO(3) 是三维旋转矩阵的集合，配以矩阵乘法：

$$\mathrm{SO}(3)=\{R\in {\mathbb{R}}^{3\times 3}\mid R^{T}R=I,\det (R)=1\}$$

白话翻译：SO(3) 就是"所有合法旋转矩阵组成的集合"，其中"合法"意味着行（列）两两正交且行列式为正一，确保旋转不带拉伸或镜像。

SO(3) 是一个三维流形——在任意旋转矩阵附近，局部看起来像三维欧氏空间 ${\mathbb{R}}^3$，但整体上是弯曲的，不具备全局线性结构。

2.2 李代数 so(3)

SO(3) 在恒等元 $I$ 处的切空间（即局部线性近似）称为李代数，记作 so(3)。它与 ${\mathbb{R}}^3$ 同构：每个三维向量 $\mathbf{\varphi }=\theta \hat{\mathbf{\omega }}\in {\mathbb{R}}^3$ 对应一个反对称矩阵：

[ϕ]×=(0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10)[\\boldsymbol{\\phi}]_\\times = \\begin{pmatrix} 0 & -\\phi_3 & \\phi_2 \\\\ \\phi_3 & 0 & -\\phi_1 \\\\ -\\phi_2 & \\phi_1 & 0 \\end{pmatrix}[ϕ]×​=​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​​

白话翻译：李代数 so(3) 就是"旋转轴 × 旋转角"这一三维向量，配以反对称矩阵的外壳。它是线性的（可以相加、缩放），因此可以在其中做卡尔曼增量的加法。

so3_math.h 中的宏 SKEW_SYM_MATRX(v) 正是将向量填入上述反对称矩阵格式：

// include/utils/so3_math.h:7
#define SKEW_SYM_MATRX(v) 0.0, –v[2], v[1], v[2], 0.0, –v[0], –v[1], v[0], 0.0

使用时展开为 K << SKEW_SYM_MATRX(r_axis)，即将三维向量 r_axis 填入 3×3 反对称矩阵 K。

2.3 指数映射 Exp：so(3) → SO(3)

给定李代数元素 $\mathbf{\varphi }=\theta \hat{\mathbf{\omega }}$（$\theta$ 为旋转角，$\hat{\mathbf{\omega }}$ 为单位旋转轴），指数映射由 Rodrigues 公式给出：

$$\mathrm{Exp}(\mathbf{\varphi })=I+\sin \theta [\hat{\mathbf{\omega }}{]}_{\times }+(1-\cos \theta )[\hat{\mathbf{\omega }}{]}_{\times }^2$$

白话翻译：给定一个"旋转轴与角度"的向量，Rodrigues 公式将其精确地转换为旋转矩阵，结果自动满足 $R^{T}R=I$，无需额外归一化。

特殊情形：当 $\theta \to 0$ 时，$\sin \theta \approx \theta$，$(1-\cos \theta )\approx 0$，公式退化为 $\mathrm{Exp}(\mathbf{\varphi })\approx I+[\mathbf{\varphi }{]}_{\times }$，即一阶线性近似——这正是在增量很小时，旋转可以近似"相加"的直觉根源。

so3_math.h 提供三个 Exp 重载，最常用的是标量参数版（src/LIVMapper.cpp 中的 operator+ 调用的即此版本）：

// include/utils/so3_math.h:44
template <typename T>
Eigen::Matrix<T,3,3> Exp(const T &v1, const T &v2, const T &v3)
{
T norm = sqrt(v1*v1 + v2*v2 + v3*v3); // θ = ||φ||
if (norm > 0.00001)
{
T r_ang[3] = {v1/norm, v2/norm, v3/norm}; // ω̂
Eigen::Matrix<T,3,3> K;
K << SKEW_SYM_MATRX(r_ang); // [ω̂]×
return Eye3 + sin(norm)*K + (1.0–cos(norm))*K*K; // Rodrigues
}
else { return Eye3; } // 小角度退化：直接返回单位阵
}

norm > 0.00001 的分支保护避免了 $\theta =0$ 时的除零；实测中 ESIKF 迭代收敛后增量通常在 $10^{-3}$ 量级，多数情况走正常分支，极少数最终收敛帧走退化分支返回 $I$。

2.4 对数映射 Log：SO(3) → so(3)

指数映射的逆是对数映射：给定旋转矩阵 $R$，提取其旋转角与轴：

$$\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right),\mathrm{Log}(R)=\frac{\theta }{2\sin \theta }(R-R^T{)}^{\vee }$$

其中 $(\cdot {)}^{\vee }$ 从反对称矩阵提取三维向量（与 hat 算子互逆）。

白话翻译：Log 从旋转矩阵中"读出"旋转轴与角度，还原为三维向量，以便在李代数中做差。

// include/utils/so3_math.h:62
template <typename T>
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T,3,3> &R)
{
T theta = (R.trace() > 3.0 – 1e-6) ? 0.0
: acos(0.5 * (R.trace() – 1)); // θ = arccos((tr(R)-1)/2)
Eigen::Matrix<T,3,1> K(R(2,1)–R(1,2), // (R-Rᵀ) 的反对称部分 vee
R(0,2)–R(2,0),
R(1,0)–R(0,1));
return (abs(theta) < 0.001) ? (0.5*K)
: (0.5*theta/sin(theta)*K);
}

两处分支保护：R.trace() > 3.0 – 1e-6 处理 $\theta \approx 0$ 的近恒等旋转（直接返回零向量）；abs(theta) < 0.001 处理小角度时 $\theta /\sin \theta$ 的数值不稳定（以泰勒一阶近似 $\approx 1$ 代替）。

图 1　SO(3) 李群与 so(3) 李代数的对应关系：Exp（绿色实线）将三维向量映射为旋转矩阵，Log（橙色虚线）为其逆；两映射均已在 so3_math.h 中实现

三、右扰动：为什么选它

拥有 Exp/Log 后，如何在 SO(3) 上定义"加一个微小增量"？有两种自然选择。

左扰动：在旋转矩阵左侧乘以增量的指数映射

$$R_{\mathrm{new}}=\mathrm{Exp}(\delta \mathbf{\varphi })\cdot R$$

物理含义：$\delta \mathbf{\varphi }$ 描述的是**世界坐标系（全局坐标系）**中的微小旋转。

右扰动：在旋转矩阵右侧乘以增量的指数映射

$$R_{\mathrm{new}}=R\cdot \mathrm{Exp}(\delta \mathbf{\varphi })$$

物理含义：$\delta \mathbf{\varphi }$ 描述的是**载体坐标系（局部坐标系）**中的微小旋转。

两者在数学上均自洽，但对 ESIKF 的雅可比矩阵推导有不同影响。选择右扰动时，旋转点 $\mathbf{p}$ 在世界坐标系中的坐标 $R\mathbf{p}$ 对扰动 $\delta \mathbf{\varphi }$ 的偏导数为：

$$\frac{\partial (R\mathbf{p})}{\partial \delta \mathbf{\varphi }}=-[R\mathbf{p}{]}_{\times }$$

白话翻译：偏导数等于"负的反对称矩阵作用于已变换的点"，形式简洁，直接对应 ESIKF 中 $H$ 矩阵的 $[-{\mathbf{p}}_w{]}_{\times }$ 项（第 7 篇推导雅可比时将详细展开）。相比之下，左扰动的偏导数需要引入伴随矩阵进行坐标系转换，表达式更繁琐。

这正是 FAST-LIVO2 选择右扰动的根本原因：雅可比简洁，与 ESIKF 的 H 矩阵直接对应，无需额外转换。

图 2　左扰动与右扰动的对比：FAST-LIVO2 选择右扰动，因其使点到面雅可比具有 -[Rp]× 的简洁形式，与 ESIKF 的 H 矩阵直接对应

四、源码精读：operator+ 与 operator-

确立了右扰动模型后，再看 StatesGroup 的流形加减运算符，就能理解每一行的含义。

4.1 流形加法 operator+：更新状态

operator+ 接受一个 19 维欧氏增量 state_add，将其"加"到当前状态上（include/common_lib.h:167）：

// include/common_lib.h:167
StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
{
StatesGroup a;
// ① 姿态：右乘 Exp，保持 SO(3) 约束
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
// ② 位置：欧氏加法（ℝ³ 无约束）
a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
// ③ 逆曝光时间：欧氏加法（标量）
a.inv_expo_time = this->inv_expo_time + state_add(6,0);
// ④–⑦ 速度、陀螺零偏、加计零偏、重力：均为欧氏加法
a.vel_end = this->vel_end + state_add.block<3,1>(7,0);
a.bias_g = this->bias_g + state_add.block<3,1>(10,0);
a.bias_a = this->bias_a + state_add.block<3,1>(13,0);
a.gravity = this->gravity + state_add.block<3,1>(16,0);
a.cov = this->cov;
return a;
}

关键点逐条解析：

① 姿态的右乘更新

this->rot_end * Exp(…) 正是右扰动模型 $R_{\mathrm{new}}=R\cdot \mathrm{Exp}(\delta \mathbf{\varphi })$ 的代码实现。state_add 的前三维（索引 0、1、2）是 ESIKF 解算出的姿态增量 $\delta \mathbf{\varphi }$，Exp 将其转为旋转矩阵后右乘到当前姿态上。结果自动满足 $R^{T}R=I$，无需任何后处理归一化。

②–⑦ 欧氏分量的直接加法

位置、速度、零偏、重力向量均位于 ${\mathbb{R}}^3$ 或标量空间，不存在约束，直接加法合法。state_add.block<3,1>(3,0) 等 Eigen 块操作以列向量形式提取对应区间的增量。

③ 逆曝光时间的特殊性

inv_expo_time 是第 1 篇提及的 FAST-LIVO2 独有亮点——将相机曝光时间的倒数作为状态量在线估计。它是一个正实数标量，其加法完全合法；ESIKF 通过光度残差对其求导，约束该量朝着使图像亮度与预测一致的方向更新。

原地更新版 operator+=（common_lib.h:182）逻辑完全相同，仅改为直接修改 this 而非返回新对象，在 ESIKF 的 state_ += solution 中被调用（voxel_map.cpp:474）：

// src/voxel_map.cpp:474
state_ += solution; // 调用 operator+=，右扰动更新 rot_end，其余直接加

4.2 流形减法 operator-：计算误差向量

operator- 计算两个状态之间的"差"，结果是一个 19 维欧氏向量，用于 ESIKF 的残差计算（include/common_lib.h:194）：

// include/common_lib.h:194
Matrix<double, DIM_STATE, 1> operator–(const StatesGroup &b)
{
Matrix<double, DIM_STATE, 1> a;
M3D rotd(b.rot_end.transpose() * this->rot_end); // R_b^T · R_a ∈ SO(3)
a.block<3,1>(0,0) = Log(rotd); // 对数映射还原为 ℝ³ 向量
a.block<3,1>(3,0) = this->pos_end – b.pos_end;
a(6,0) = this->inv_expo_time – b.inv_expo_time;
a.block<3,1>(7,0) = this->vel_end – b.vel_end;
a.block<3,1>(10,0)= this->bias_g – b.bias_g;
a.block<3,1>(13,0)= this->bias_a – b.bias_a;
a.block<3,1>(16,0)= this->gravity – b.gravity;
return a;
}

姿态差的计算分两步：

这与 ESIKF 的线性化框架完全吻合：operator- 提供的 19 维向量就是误差状态（error state），ESIKF 在误差状态空间中建立高斯分布并做卡尔曼更新，operator+ 则将更新后的误差状态重新注入名义状态（nominal state）。

voxel_map.cpp:470 中的 auto vec = state_propagat – state_ 正是这一调用——计算 IMU 传播状态与当前优化状态之间的差，作为先验残差送入卡尔曼增益计算：

// src/voxel_map.cpp:470
auto vec = state_propagat – state_; // operator-，姿态差经 Log 映射
VD(DIM_STATE) solution = K_1.block<DIM_STATE,6>(0,0) * HTz
+ vec.block<DIM_STATE,1>(0,0)
– G.block<DIM_STATE,6>(0,0) * vec.block<6,1>(0,0);
state_ += solution; // operator+=，右扰动更新

这六行是 ESIKF 状态更新的完整闭环：计算先验残差 → 加权融合观测残差 → 更新状态。第 6 篇将展开完整的 ESIKF 数学推导，届时这些运算的来源将更加清晰。

图 3　operator+ 对 19 维状态的分类更新：仅姿态分量经 Exp 右乘（保持 SO(3) 约束），其余 16 维均为欧氏直接加法

五、StatesGroup 初始化中的协方差设计

在理解 operator+ 之后，还有一处细节值得关注：StatesGroup 的默认构造函数中，协方差矩阵的初始化并非均匀的（include/common_lib.h:128）：

// include/common_lib.h:137
this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV; // 全局初始协方差 0.01
this->cov(6, 6) = 0.00001; // 逆曝光时间初始不确定度极小
this->cov.block<9,9>(10,10) = MD(9,9)::Identity() * 0.00001; // 零偏与重力初始极稳定

三条语句反映了对系统初始不确定度的先验判断：

• 全局 INIT_COV = 0.01：姿态、位置、速度在启动时有较大不确定度（$\sigma \approx 0.1$），合理；
• cov(6,6) = 1e{-5}：逆曝光时间初始设为近确定值（初始曝光估计通过 exposure_time_init 参数指定），不确定度极小；
• 零偏与重力 1e{-5}：陀螺仪、加速度计零偏以及重力方向初始设为高置信度估计，避免滤波器启动时因零偏不确定度过大而出现震荡。

这种非均匀初始化是工程经验的体现：参数估计类状态（曝光、零偏、重力）的初始值来自标定，不确定度本身已经很小；而运动类状态（位置、速度、姿态）的初始值完全依赖 IMU 静止对齐，不确定度相对更高。

六、与 ORB-SLAM3 的对比：同一基础，不同侧重

本专栏第 1 篇提及 FAST-LIVO2 采用直接法（稀疏直接法），与 ORB-SLAM3 等特征法在视觉处理上走不同路线。但两者在旋转表示上高度一致：

• ORB-SLAM3 使用 Sophus SE(3)（特殊欧氏群），内部同样以右扰动模型推导 ESIKF/g2o 的雅可比；
• FAST-LIVO2 直接用 so3_math.h 的 Exp/Log，不依赖 Sophus 的封装，在激光-惯性系统中更轻量且对协方差传播更透明。

两者最实质的差别不在于左右扰动的选择（两者均用右），而在于观测方程的构成：ORB-SLAM3 以特征点重投影误差建立观测方程，FAST-LIVO2 以光度误差（视觉）和点到面距离（激光）建立观测方程。这些差异将在第 7、13 篇展开。

七、小结与下篇预告

本篇建立了 ESIKF 与后续所有数学推导的流形基础，核心结论：

下一篇将进入 IMU 运动模型与离散传播：连续时间运动方程如何离散化、协方差如何通过 $F_x$/$F_w$ 矩阵传播，以及 IMU_Processing.cpp 中前向传播的源码逐段解读。

下一篇：《FAST-LIVO2 源码精读（五）：IMU 运动模型、离散传播与去畸变原理》