VIO边缘化与NfrMapper：从定位到建图的信息转换

1. 从VIO边缘化到后端建图：NfrMapper原理与实现解析

在视觉惯性里程计（VIO）系统中，边缘化是一个关键但常被忽视的环节。传统VIO系统在进行边缘化处理后，往往直接将先验信息丢弃，导致后续建图过程无法充分利用这些宝贵的信息。NfrMapper的创新之处在于，它提出了一套系统性的方法，将VIO边缘化后留下的线性高斯先验转化为可继续优化的非线性因子，实现了从实时定位到高质量建图的无缝衔接。

1.1 核心问题定义

VIO系统在运行过程中会不断边缘化旧的状态变量，产生所谓的"边缘化先验"。这个先验通常以信息矩阵H和一阶项b的形式存在，包含了被边缘化变量与保留变量之间的约束关系。传统做法直接将这些先验作为固定约束使用，导致两个主要问题：

1. 信息矩阵H通常是稠密的，随着时间推移会变得越来越庞大，严重影响计算效率
2. 固定线性化点导致无法与新的视觉观测进行联合优化

NfrMapper的核心创新点在于提出了一种"信息重参数化"方法，将稠密的高斯先验蒸馏为稀疏的几何因子，主要包括：

• 相对位姿因子（Relative Pose Factor）
• 滚转/俯仰因子（Roll/Pitch Factor）

1.2 整体流程概述

NfrMapper的处理流程可以概括为以下五个关键步骤：

1. 边缘化数据获取：从VIO系统获取边缘化后的信息矩阵H和一阶项b
2. 协方差恢复：通过矩阵求逆运算得到状态变量的协方差矩阵Σ
3. 因子提取：从协方差矩阵中提取相对位姿因子和滚转/俯仰因子
4. 视觉重建：对当前帧进行特征提取、匹配和三角化，建立新的地图点
5. 联合优化：将提取的几何因子与新的视觉重投影误差一起进行BA优化

数学上，最终的优化目标函数可以表示为：
E = E_reproj + E_relpose + E_roll/pitch

其中E_reproj表示视觉重投影误差，E_relpose表示相对位姿因子误差，E_roll/pitch表示滚转/俯仰因子误差。

2. 边缘化后的数学本质与信息提取

2.1 边缘化结果的数学表示

VIO系统在进行边缘化操作后，会得到一个关于状态增量δx的二次型能量函数：
E(δx) ≈ 1/2 δxᵀHδx + bᵀδx

这对应于一个高斯分布：
p(δx) ∝ exp(-1/2 δxᵀHδx - bᵀδx)

其中H是信息矩阵（Hessian矩阵），b是一阶项。通过求逆可以得到协方差矩阵：
Σ = H⁻¹

这里需要特别强调的是，VIO边缘化后提供的不只是简单的约束条件，更重要的是它完整保留了状态变量之间的不确定性结构。这种不确定性结构对于后续的建图优化至关重要。

2.2 从协方差到因子的转换原理

NfrMapper的核心创新在于它实现了一种"信息蒸馏"过程，将稠密的协方差矩阵转换为稀疏的几何因子。这个过程本质上是一个信息重参数化（information reparameterization）问题。

具体来说，我们需要完成从"状态空间的不确定性"到"观测空间的约束"的转换。这种转换之所以可行，基于以下几个关键观察：

1. 边缘化本身已经是局部高斯近似，我们只是将这种近似转换为另一种更适合建图的形式
2. 提取的是最稳定的信息（如滚转/俯仰、相对位姿），避开了绝对位置和偏航角等不可观或弱观测量
3. 后续的视觉BA优化会进一步修正和细化这些约束

2.3 为什么需要这种转换

直接使用原始的信息矩阵H会带来几个严重问题：

1. 稠密性问题：H矩阵通常是完全稠密的，随着时间推移会变得越来越庞大，计算和存储成本急剧上升
2. 线性化点依赖：H矩阵依赖于特定的线性化点，当系统状态发生较大变化时，这种近似会变得不准确
3. 可解释性差：稠密的H矩阵难以直观理解和调试
4. 扩展困难：难以与新的观测进行有效融合

相比之下，转换后的几何因子具有以下优势：

• 稀疏性：每个因子只涉及少量变量
• 可解释性：每个因子都有明确的几何意义
• 可重复线性化：可以在新的线性化点重新计算
• 易于扩展：可以方便地与其他因子组合

3. 相对位姿因子(RelPoseFactor)的构造

3.1 基本定义与数学表示

相对位姿因子是NfrMapper中最重要的因子类型之一。给定两个关键帧i和j，它们的位姿分别为T_i和T_j∈SE(3)，我们首先定义它们的相对位姿：
T_ij = T_i⁻¹T_j

在理想情况下，这个相对位姿应该与从VIO边缘化信息中恢复的相对位姿一致。因此，我们可以定义残差：
r = log(T_i⁻¹T_j · T_ij⁻¹)

其中log: SE(3)→ℝ⁶是李代数对数映射，将SE(3)中的位姿转换映射到切空间中的6维向量。这个残差r实际上衡量了当前估计的相对位姿与先验相对位姿之间的差异。

3.2 SE(3)对数映射详解

SE(3)的对数映射是将位姿矩阵转换为其对应的李代数元素的过程。给定一个位姿矩阵：
T = [R t; 0 1]

其对数映射结果为：
log(T) = [ω ρ]ᵀ ∈ ℝ⁶

其中ω∈ℝ³是旋转部分对应的角轴向量，ρ∈ℝ³是平移部分（考虑了旋转耦合）。具体计算过程如下：

1. 旋转部分ω通过SO(3)的对数映射计算：
   ω = log(R)

2. 平移部分ρ通过左雅可比矩阵的逆计算：
   ρ = J⁻¹t

其中J是SO(3)的左雅可比矩阵，定义为：
J = I + (1-cosθ)/θ² [ω]× + (θ-sinθ)/θ³ [ω]ײ

这里θ=‖ω‖，[ω]_×是ω的反对称矩阵。

3.3 雅可比矩阵推导

为了将状态协方差投影到残差空间，我们需要计算残差关于状态变量的雅可比矩阵。考虑对位姿T_i和T_j施加微小扰动：
T_i ← T_i·exp(δξ_i)
T_j ← T_j·exp(δξ_j)

其中δξ_i, δξ_j∈se(3)是微小扰动。通过一阶泰勒展开，我们可以得到残差的线性近似：
r ≈ r₀ + J_iδξ_i + J_jδξ_j

经过推导（详见附录），可以得到雅可比矩阵的解析表达式：
J_i = -Adj(T_ij⁻¹)
J_j = I

其中Adj(T)是SE(3)的伴随矩阵，定义为：
Adj(T) = [R 0; [t]_×R R]

3.4 信息矩阵投影

有了雅可比矩阵后，我们可以将状态空间的信息矩阵Λ_ij投影到残差空间：
Λ_r = JΛ_ijJᵀ

其中Λ_ij是从全局协方差矩阵Σ中提取的关于位姿i和j的子块的信息矩阵（Λ_ij = Σ_ij⁻¹）。

最终，相对位姿因子的误差项可以表示为：
E = rᵀΛ_r r

4. 滚转/俯仰因子(RollPitchFactor)的构造

4.1 基本定义与数学表示

滚转/俯仰因子是NfrMapper中另一个重要因子类型。它主要用于约束关键帧的绝对姿态中的滚转和俯仰角，这两个角度在VIO系统中通常可以通过重力观测很好地确定。

给定一个关键帧的位姿T_i，我们首先提取其旋转矩阵R_i。滚转(roll)和俯仰(pitch)可以通过旋转矩阵的元素直接计算：
roll = atan2(R_i(3,2), R_i(3,3))
pitch = atan2(-R_i(3,1), √(R_i(3,2)² + R_i(3,3)²))

因此，我们可以定义残差：
r = [roll; pitch] - [roll_meas; pitch_meas]

其中[roll_meas; pitch_meas]是从VIO边缘化信息中恢复的测量值。

4.2 雅可比矩阵计算

滚转/俯仰残差关于状态变量的雅可比矩阵计算相对简单。我们只需要计算滚转和俯仰关于位姿T_i的导数。由于滚转和俯仰只与旋转部分有关，我们可以忽略平移部分。

具体来说，对于位姿T_i的扰动δξ_i = [δρ_i; δϕ_i]，其中δρ_i是平移扰动，δϕ_i是旋转扰动，我们有：
∂r/∂δϕ_i = [∂roll/∂δϕ_i; ∂pitch/∂δϕ_i]

而∂roll/∂δϕ_i和∂pitch/∂δϕ_i可以通过链式法则和旋转矩阵的导数公式计算得到。

4.3 信息矩阵投影

与相对位姿因子类似，我们可以将状态空间的信息矩阵Λ_ii投影到滚转/俯仰残差空间：
Λ_r = JΛ_iiJᵀ

其中J是滚转/俯仰残差关于状态变量的雅可比矩阵，Λ_ii是位姿i的边际信息矩阵。

最终，滚转/俯仰因子的误差项可以表示为：
E = rᵀΛ_r r

5. 实现细节与工程考量

5.1 协方差矩阵的稳健求逆

在实际实现中，从信息矩阵H求取协方差矩阵Σ=H⁻¹可能会遇到数值不稳定的问题，特别是当H矩阵条件数较大时。为了提高数值稳定性，可以采用以下策略：

1. 添加小的正则化项：H_reg = H + εI，其中ε是一个小的正数
2. 使用Cholesky分解结合伪逆：对H进行Cholesky分解H=LLᵀ，然后计算Σ=L⁻ᵀL⁻¹
3. 对于特别大的矩阵，可以采用分块求逆的方法

5.2 关键帧选择策略

在构造相对位姿因子时，关键帧的选择至关重要。一个好的关键帧选择策略应该考虑以下因素：

1. 时间基线：选择时间上相隔不远的关键帧，确保有足够的共同观测
2. 空间分布：确保关键帧在空间上分布均匀，避免局部过密
3. 信息量：选择能够提供新信息的帧，避免冗余

常用的策略包括：

• 固定间隔选择
• 基于视差或特征变化的自适应选择
• 基于信息增益的优化选择

5.3 因子权重调整

从协方差矩阵提取的几何因子需要与视觉重投影误差进行联合优化，因此需要考虑不同因子之间的相对权重。在实际实现中，可以：

1. 对信息矩阵进行适当的缩放，使其与重投影误差的尺度匹配
2. 引入鲁棒核函数（如Huber核）处理异常值
3. 根据因子不确定性动态调整权重

5.4 与视觉BA的集成

NfrMapper提取的几何因子最终需要与视觉重投影误差一起进行BA优化。在实现时需要注意：

1. 参数化一致性：确保所有因子使用相同的参数化方式（如李代数或四元数）
2. 线性求解器选择：由于问题通常是稀疏的，可以使用稀疏Cholesky分解或共轭梯度法
3. 边缘化处理：在BA过程中也需要进行边缘化，要注意与前端VIO的边缘化协调

6. 实际应用与性能分析

6.1 系统架构设计

在实际系统中，NfrMapper通常作为建图模块的一部分，与前端VIO系统协同工作。典型的系统架构包括：

1. 前端VIO：负责实时位姿估计和特征跟踪
2. 边缘化管理：处理滑窗优化和边缘化操作
3. 因子提取：实现NfrMapper的核心算法
4. 局部BA：整合几何因子和视觉观测进行优化
5. 全局地图管理：维护和优化全局地图

6.2 计算效率分析

NfrMapper的主要计算开销来自以下几个方面：

1. 协方差矩阵求逆：O(n³)复杂度，n是状态变量维度
2. 因子提取：主要是矩阵乘法，复杂度取决于关键帧数量
3. BA优化：通常是稀疏问题，复杂度与地图点和关键帧数量相关

通过合理的实现和优化（如并行计算、增量更新等），NfrMapper可以在保持精度的同时满足实时性要求。

6.3 精度与鲁棒性评估

在实际应用中，NfrMapper相比传统方法展现出以下优势：

1. 更高的精度：通过充分利用边缘化信息，减少了信息损失
2. 更好的一致性：几何因子保持了变量之间的约束关系
3. 更强的鲁棒性：对视觉观测的丢失或退化更不敏感

定量评估通常采用标准数据集（如EuRoC、TUM-VI等）进行，指标包括绝对轨迹误差（ATE）、相对位姿误差（RPE）等。

7. 扩展与变种

7.1 与其他传感器的融合

NfrMapper的基本框架可以扩展到多传感器融合场景：

1. 激光雷达：可以提取点云配准约束作为额外因子
2. GPS/RTK：在室外场景可以添加绝对位置约束
3. 轮速计：提供平面运动约束

7.2 动态环境适应

在动态环境中，传统的VIO和建图方法可能会失效。NfrMapper可以通过以下方式增强：

1. 动态物体检测与剔除
2. 多假设跟踪与管理
3. 自适应因子权重调整

7.3 大规模场景应用

对于大规模场景，可以考虑以下扩展：

1. 分层建图：将地图分为多个层次管理
2. 位姿图优化：在全局层面使用位姿图优化
3. 子地图技术：构建并优化局部子地图

8. 常见问题与解决方案

8.1 协方差矩阵奇异性问题

问题描述：在边缘化过程中，信息矩阵H可能变得奇异或接近奇异，导致求逆困难。

解决方案：

1. 添加正则化项
2. 使用伪逆代替常规逆
3. 检查并修复系统可观性

8.2 因子冲突问题

问题描述：不同因子之间可能存在冲突，导致优化不稳定。

解决方案：

1. 引入鲁棒核函数
2. 动态调整因子权重
3. 使用协方差交集等方法融合多源信息

8.3 尺度漂移问题

问题描述：在纯视觉惯性系统中，尺度可能随时间漂移。

解决方案：

1. 定期进行尺度校正
2. 引入绝对尺度观测（如已知尺寸物体）
3. 使用双目或深度相机提供尺度信息

9. 实现建议与最佳实践

9.1 数学库选择

实现NfrMapper需要可靠的数学库支持，推荐：

1. 线性代数：Eigen
2. 李群操作：Sophus
3. 非线性优化：g2o、GTSAM或Ceres

9.2 数值稳定性技巧

1. 使用四元数进行旋转插值和平均
2. 对极小数和大数进行特殊处理
3. 实现完善的异常检测和处理机制

9.3 调试与可视化

1. 实现因子图可视化工具
2. 记录并分析优化过程中的残差变化
3. 使用仿真数据验证核心算法

10. 总结与展望

NfrMapper通过创新的信息重参数化方法，成功地将VIO边缘化产生的稠密高斯先验转换为稀疏的几何因子，实现了实时系统与高质量建图的解耦。这种方法不仅具有坚实的数学基础，而且在工程实践中也展现出了优异的性能。

未来可能的改进方向包括：

1. 更智能的关键帧选择和因子提取策略
2. 与深度学习方法的结合
3. 更高效的大规模建图技术
4. 动态环境的自适应处理

在实际应用中，我发现以下几点经验特别值得分享：

1. 因子权重的自动调整对系统性能影响很大
2. 保持前端VIO和建图模块之间的一致性至关重要
3. 合理的边缘化策略可以显著提高系统鲁棒性
4. 可视化工具对于调试和优化不可或缺