在FAST-LIO这类紧耦合的激光惯性里程计系统中状态向量通常包含位置ℝ³中的平移和姿态SO(3)中的旋转两种不同类型的量。这两类量的加法运算是互不相容的平移可以用普通的向量加法直接叠加旋转却需要通过矩阵乘法或四元数乘法组合。这种类型不一致使得状态估计中的线性化和滤波更新变得非常棘手。FAST-LIO的解决方式是通过定义广义加减法⊞和⊟来统一状态向量各组成部分的更新方式从而将流形上的非线性问题转化为线性空间中的常规卡尔曼滤波问题。一、从旋转矩阵到流形旋转的表示方式存在一个根本性的矛盾为了满足运算的封闭性旋转矩阵SO(3)和四元数需要用过参数化的形式9维或4维来描述仅有3个自由度的旋转为了用最少的参数表示欧拉角会遭遇万向锁带来的奇异性问题。然而状态估计中的卡尔曼滤波又要求状态量在向量空间上定义良好的加法运算。流形提供了解决这一矛盾的数学框架。SO(3)是一个三维流形局部具有欧几里得空间的结构。这意味着在旋转矩阵附近的一个小范围内可以用一个三维向量即李代数来参数化旋转的“扰动”。这个局部到整体的桥梁就是指数映射和对数映射。指数映射将李代数中的旋转向量映射到SO(3)上的旋转矩阵。当旋转向量 φ 的模长为 θ 时对应的旋转矩阵可由罗德里格斯公式给出。对数映射则是其逆操作将一个旋转矩阵映射回李代数中的旋转向量。将旋转矩阵 R 与一个小的旋转向量 r 进行“加法”实际上是通过指数映射完成的textR ⊞ r R · Exp(r)将 r 经由指数映射转变为旋转矩阵再与原旋转矩阵相乘。这就是将流形上的点“向前移动一小步”的正确方法。二、广义加减法的精确定义设 M 是一个 n 维的流形例如 SO(3)Rⁿ 是其切空间。FAST-LIO论文中引入了两个操作符广义加法⊞text⊞ : M × Rⁿ → M其含义是在流形上的一个点 x 处施加一个小的切空间扰动 u得到一个新的流形上的点 y x ⊞ u。广义减法⊟text⊟ : M × M → Rⁿ其含义是计算流形上两点之间的差异并用切空间中的一个向量来表述。当流形取具体的几何对象时这些运算符的含义变得非常直观。当 M SO(3)旋转矩阵时R ⊞ r R · Exp(r)R1 ⊟ R2 Log(R2^T · R1)。当 M Rⁿ平移向量时a ⊞ b a ba ⊟ b a - b。对于复合流形例如 FAST-LIO 的状态向量 x (R, p, v, bω, ba) ∈ SO(3) × R¹⁵广义加减法也被相应地定义为在各个子空间上独立执行上述操作再将结果组合起来。三、将流形上的误差传递简化为欧几里得形式广义加减法的一个重要性质使得流形上的误差传递能够以向量空间中的常规方式来表述即text(x ⊞ u) ⊟ x ux ⊞ (y ⊟ x) y对任意 x, y ∈ M 和 u ∈ Rⁿ 成立。这一性质表明在局部切空间中进行加法运算然后“投影”回流形再通过广义减法将该点“减”回原点的切空间得到的扰动向量与最初施加的完全一致。在FAST-LIO中每个时刻的状态变量是流形 M 上的点如位姿而状态估计的误差量误差状态被定义在切空间 Rⁿ 中。真实状态 名义状态 ⊞ 误差状态。通过在切空间线性空间中完成误差状态的所有线性运算如卡尔曼滤波的预测和更新然后在需要时通过广义加法⊞将误差叠加到名义状态上获得流形上新的状态点。这种方式既避免了直接对旋转矩阵或四元数做加法这一操作无意义又充分利用了切空间的优良线性性质来进行状态估计。在ESKF的理论框架中误差状态变量可以在原点附近取极小的值线性化近似非常精准离奇点较远雅可比矩阵的计算也因此大为简化。四、源代码实现FAST-LIO2中的状态估计依赖一个名为IKFoMIterative Kalman Filter on Manifold的工具箱该工具箱的核心就是基于广义加减法构建了一套完整的流形运算体系。1. 状态向量定义use-ikfom.hpp该文件使用宏构建了一个名为 state_ikfom 的复合流形结构体。该结构体包含位置、姿态SO3、速度、IMU零偏以及重力向量等。同时该宏为该结构体自动生成了 boxplus 和 boxminus 方法能够正确处理其中每个子流形的运算。2. boxplus操作实现在流形运算的头文件中boxplus函数为实现广义加法运算而实现。对于旋转部分SO3它使用指数映射将李代数扰动三维向量转换为旋转矩阵然后与原旋转相乘对于平移速度等欧氏部分则直接使用向量加法。一种实现方式的参考代码片段为cppstate_ikfom boxplus(state_ikfom x, Eigen::Matrixdouble, 24, 1 f_) {state_ikfom x_r;x_r.pos x.pos f_.block3, 1(0, 0); // 平移直接向量加法x_r.rot x.rot * Sophus::SO3::exp(f_.block3, 1(3, 0)); // 旋转使用指数映射return x_r;}旋转部分的处理体现了广义加法的核心思想——将线性空间中的扰动通过指数映射提升回流形再进行组合。3. boxminus操作实现在状态更新和观测方程线性化中需要计算流形上两点之间的误差向量。boxminus函数通过对数映射实现这一操作textVec point1 ⊟ point2 Log(point2^{-1} ∘ point1)它返回的 Vec 直接就是定义在流形切空间上的误差状态可以直接用于滤波器的更新方程。五、参考资料FAST-LIO原论文Xu, Wei, and Fu Zhang. FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter.IEEE Robotics and Automation Letters, 2021. arXiv版本《视觉SLAM十四讲》高翔等著第3-4讲李群与李代数IKFoM工具包代码解读系列基于Boost Preprocessor的Manifold Toolkit (MTK) 通用流形类构建的系列博客知乎专栏《FAST-LIO论文简要介绍》作者严国陶提供了对论文核心概念的体系化解读FAST-LIO官方代码仓库https://github.com/hku-mars/FAST_LIO和https://github.com/hku-mars/FAST_LIO2六、推荐学习路径先回顾《视觉SLAM十四讲》第3-4讲李群与李代数的基本概念理解旋转矩阵、四元数、指数映射及对数映射的基本内容。阅读FAST-LIO论文重点关注“操作符”的定义及ESKF离散化过程的公式特别是式1、4、5、22等。结合官方代码的 use-ikfom.hpp 文件和IKFoM工具箱中与流形相关的头文件理解 boxplus 和 boxminus 函数在状态预测和更新步骤中如何被调用。参考CSDN博客《FAST-LIO论文梳理及公式理解》通过其中的图文对照加深对公式和代码之间映射关系的理解。初步掌握概念后在仿真环境中设置不同的传感器噪声水平观察算法实际工作时的收敛过程加深对理论的直观认识。
流形上的优化：SO(3)与SE(3)的广义加减法在FAST-LIO中如何简化状态估计
在FAST-LIO这类紧耦合的激光惯性里程计系统中状态向量通常包含位置ℝ³中的平移和姿态SO(3)中的旋转两种不同类型的量。这两类量的加法运算是互不相容的平移可以用普通的向量加法直接叠加旋转却需要通过矩阵乘法或四元数乘法组合。这种类型不一致使得状态估计中的线性化和滤波更新变得非常棘手。FAST-LIO的解决方式是通过定义广义加减法⊞和⊟来统一状态向量各组成部分的更新方式从而将流形上的非线性问题转化为线性空间中的常规卡尔曼滤波问题。一、从旋转矩阵到流形旋转的表示方式存在一个根本性的矛盾为了满足运算的封闭性旋转矩阵SO(3)和四元数需要用过参数化的形式9维或4维来描述仅有3个自由度的旋转为了用最少的参数表示欧拉角会遭遇万向锁带来的奇异性问题。然而状态估计中的卡尔曼滤波又要求状态量在向量空间上定义良好的加法运算。流形提供了解决这一矛盾的数学框架。SO(3)是一个三维流形局部具有欧几里得空间的结构。这意味着在旋转矩阵附近的一个小范围内可以用一个三维向量即李代数来参数化旋转的“扰动”。这个局部到整体的桥梁就是指数映射和对数映射。指数映射将李代数中的旋转向量映射到SO(3)上的旋转矩阵。当旋转向量 φ 的模长为 θ 时对应的旋转矩阵可由罗德里格斯公式给出。对数映射则是其逆操作将一个旋转矩阵映射回李代数中的旋转向量。将旋转矩阵 R 与一个小的旋转向量 r 进行“加法”实际上是通过指数映射完成的textR ⊞ r R · Exp(r)将 r 经由指数映射转变为旋转矩阵再与原旋转矩阵相乘。这就是将流形上的点“向前移动一小步”的正确方法。二、广义加减法的精确定义设 M 是一个 n 维的流形例如 SO(3)Rⁿ 是其切空间。FAST-LIO论文中引入了两个操作符广义加法⊞text⊞ : M × Rⁿ → M其含义是在流形上的一个点 x 处施加一个小的切空间扰动 u得到一个新的流形上的点 y x ⊞ u。广义减法⊟text⊟ : M × M → Rⁿ其含义是计算流形上两点之间的差异并用切空间中的一个向量来表述。当流形取具体的几何对象时这些运算符的含义变得非常直观。当 M SO(3)旋转矩阵时R ⊞ r R · Exp(r)R1 ⊟ R2 Log(R2^T · R1)。当 M Rⁿ平移向量时a ⊞ b a ba ⊟ b a - b。对于复合流形例如 FAST-LIO 的状态向量 x (R, p, v, bω, ba) ∈ SO(3) × R¹⁵广义加减法也被相应地定义为在各个子空间上独立执行上述操作再将结果组合起来。三、将流形上的误差传递简化为欧几里得形式广义加减法的一个重要性质使得流形上的误差传递能够以向量空间中的常规方式来表述即text(x ⊞ u) ⊟ x ux ⊞ (y ⊟ x) y对任意 x, y ∈ M 和 u ∈ Rⁿ 成立。这一性质表明在局部切空间中进行加法运算然后“投影”回流形再通过广义减法将该点“减”回原点的切空间得到的扰动向量与最初施加的完全一致。在FAST-LIO中每个时刻的状态变量是流形 M 上的点如位姿而状态估计的误差量误差状态被定义在切空间 Rⁿ 中。真实状态 名义状态 ⊞ 误差状态。通过在切空间线性空间中完成误差状态的所有线性运算如卡尔曼滤波的预测和更新然后在需要时通过广义加法⊞将误差叠加到名义状态上获得流形上新的状态点。这种方式既避免了直接对旋转矩阵或四元数做加法这一操作无意义又充分利用了切空间的优良线性性质来进行状态估计。在ESKF的理论框架中误差状态变量可以在原点附近取极小的值线性化近似非常精准离奇点较远雅可比矩阵的计算也因此大为简化。四、源代码实现FAST-LIO2中的状态估计依赖一个名为IKFoMIterative Kalman Filter on Manifold的工具箱该工具箱的核心就是基于广义加减法构建了一套完整的流形运算体系。1. 状态向量定义use-ikfom.hpp该文件使用宏构建了一个名为 state_ikfom 的复合流形结构体。该结构体包含位置、姿态SO3、速度、IMU零偏以及重力向量等。同时该宏为该结构体自动生成了 boxplus 和 boxminus 方法能够正确处理其中每个子流形的运算。2. boxplus操作实现在流形运算的头文件中boxplus函数为实现广义加法运算而实现。对于旋转部分SO3它使用指数映射将李代数扰动三维向量转换为旋转矩阵然后与原旋转相乘对于平移速度等欧氏部分则直接使用向量加法。一种实现方式的参考代码片段为cppstate_ikfom boxplus(state_ikfom x, Eigen::Matrixdouble, 24, 1 f_) {state_ikfom x_r;x_r.pos x.pos f_.block3, 1(0, 0); // 平移直接向量加法x_r.rot x.rot * Sophus::SO3::exp(f_.block3, 1(3, 0)); // 旋转使用指数映射return x_r;}旋转部分的处理体现了广义加法的核心思想——将线性空间中的扰动通过指数映射提升回流形再进行组合。3. boxminus操作实现在状态更新和观测方程线性化中需要计算流形上两点之间的误差向量。boxminus函数通过对数映射实现这一操作textVec point1 ⊟ point2 Log(point2^{-1} ∘ point1)它返回的 Vec 直接就是定义在流形切空间上的误差状态可以直接用于滤波器的更新方程。五、参考资料FAST-LIO原论文Xu, Wei, and Fu Zhang. FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter.IEEE Robotics and Automation Letters, 2021. arXiv版本《视觉SLAM十四讲》高翔等著第3-4讲李群与李代数IKFoM工具包代码解读系列基于Boost Preprocessor的Manifold Toolkit (MTK) 通用流形类构建的系列博客知乎专栏《FAST-LIO论文简要介绍》作者严国陶提供了对论文核心概念的体系化解读FAST-LIO官方代码仓库https://github.com/hku-mars/FAST_LIO和https://github.com/hku-mars/FAST_LIO2六、推荐学习路径先回顾《视觉SLAM十四讲》第3-4讲李群与李代数的基本概念理解旋转矩阵、四元数、指数映射及对数映射的基本内容。阅读FAST-LIO论文重点关注“操作符”的定义及ESKF离散化过程的公式特别是式1、4、5、22等。结合官方代码的 use-ikfom.hpp 文件和IKFoM工具箱中与流形相关的头文件理解 boxplus 和 boxminus 函数在状态预测和更新步骤中如何被调用。参考CSDN博客《FAST-LIO论文梳理及公式理解》通过其中的图文对照加深对公式和代码之间映射关系的理解。初步掌握概念后在仿真环境中设置不同的传感器噪声水平观察算法实际工作时的收敛过程加深对理论的直观认识。
