1. 从VIO边缘化到后端建图:NfrMapper原理与实现解析
在视觉惯性里程计(VIO)系统中,边缘化是一个关键但常被忽视的环节。传统VIO系统在进行边缘化处理后,往往直接将先验信息丢弃,导致后续建图过程无法充分利用这些宝贵的信息。NfrMapper的创新之处在于,它提出了一套系统性的方法,将VIO边缘化后留下的线性高斯先验转化为可继续优化的非线性因子,实现了从实时定位到高质量建图的无缝衔接。
1.1 核心问题定义
VIO系统在运行过程中会不断边缘化旧的状态变量,产生所谓的"边缘化先验"。这个先验通常以信息矩阵H和一阶项b的形式存在,包含了被边缘化变量与保留变量之间的约束关系。传统做法直接将这些先验作为固定约束使用,导致两个主要问题:
- 信息矩阵H通常是稠密的,随着时间推移会变得越来越庞大,严重影响计算效率
- 固定线性化点导致无法与新的视觉观测进行联合优化
NfrMapper的核心创新点在于提出了一种"信息重参数化"方法,将稠密的高斯先验蒸馏为稀疏的几何因子,主要包括:
- 相对位姿因子(Relative Pose Factor)
- 滚转/俯仰因子(Roll/Pitch Factor)
1.2 整体流程概述
NfrMapper的处理流程可以概括为以下五个关键步骤:
- 边缘化数据获取:从VIO系统获取边缘化后的信息矩阵H和一阶项b
- 协方差恢复:通过矩阵求逆运算得到状态变量的协方差矩阵Σ
- 因子提取:从协方差矩阵中提取相对位姿因子和滚转/俯仰因子
- 视觉重建:对当前帧进行特征提取、匹配和三角化,建立新的地图点
- 联合优化:将提取的几何因子与新的视觉重投影误差一起进行BA优化
数学上,最终的优化目标函数可以表示为:
E = E_reproj + E_relpose + E_roll/pitch
其中E_reproj表示视觉重投影误差,E_relpose表示相对位姿因子误差,E_roll/pitch表示滚转/俯仰因子误差。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
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的核心创新在于它实现了一种"信息蒸馏"过程,将稠密的协方差矩阵转换为稀疏的几何因子。这个
