为什么边缘化先验的Hessian矩阵可分解得到Jacobian矩阵?

关于边缘化原理的详述可以看这篇笔记:滑动窗口与边缘化。

如上图,激光里程计因子(绿色)和imu积分因子(红色)同时约束相邻的节点。若要删除第0帧,就需要通过"边缘化"的操作把旧帧的约束传递下来,作为一个"边缘化先验因子"继续参与优化,约束窗口中剩余的所有与被边缘化变量曾有共同约束的状态变量。如下图所示:

边缘化时,挑出和要边缘化掉的量相关的因子,构建待边缘化的Hessian矩阵,并使用舒尔补做边缘化提取出先验约束因子**。** 假设为待边缘化的Hessian矩阵:

对于可以拆解为如下形式:

其中,对应要边缘化掉的。拆解的目的是通过一系列操作,把从状态量里删除掉,并将与它相关的因子的约束保留下来。借助舒尔补理论对矩阵H进行三角化,即:

化简可得:

此时不想再优化,那就只计算,可以利用等式第2行直接得到:

这一步形成的约束(上式)就叫先验因子,它包含了边缘化掉的量对剩余变量的约束关系。此时得到了关于剩余变量的先验信息矩阵和向量:

此时,先验系统满足方程:

由于优化器实际需要的是Jacobian矩阵和残差,所以需要根据和来反推出和。而所有合法的图优化因子对应的Hessian矩阵,都可以表示为某个Jacobian矩阵的转置乘以自身(将合并到中,令,则)。

反推的过程会使用特征值分解的方法,从Hessian矩阵中分解出Jacobian矩阵。特征值分解过程为:

则先验因子对应的Jacobian矩阵和残差为:

之所以敢放心地对先验因子进行特征值分解,并在后续继续使用它,其根本底气就来源于:舒尔补操作完美继承了原 Hessian 矩阵的对称半正定性(SPSD, Symmetric Positive Semi-Definite)。

对称半正定矩阵的谱分解(特征值分解)定理

对于任意n阶对称半正定矩阵 A,存在正交矩阵V(满足)和对角矩阵(其中,即非负特征值),使得:

由于所有特征值非负,我们可以定义的平方根矩阵:

显然有。将其代入谱分解公式,得:

令,则可得:

这正是我们需要的 Hessian 与 Jacobian 的关系。

的对称半正定性的证明过程如下:

1.原始矩阵 H 是对称半正定的

在基于非线性最小二乘的图优化中,我们的 Hessian 矩阵其实是近似的,它是雅可比矩阵 J 和信息矩阵的乘积:

因为协方差矩阵的逆是对称正定的,所以无论是什么样,天然就是一个对称半正定矩阵()。

原矩阵的分块形式为:

边缘化得到的先验矩阵(舒尔补)为:

2.证明:的对称性

这部分非常直观,只需对**** 求转置即可。已知是对称的,所以有:

,,

对舒尔补求转置,得:

代入对称条件,得:

结论:****绝对是对称的。

3.证明:的半正定性

要证明一个矩阵是半正定的,最经典的方法是证明它的二次型大于等于零。这个过程在状态估计中有着极强的物理意义------它代表着系统的"能量"或"误差代价"永远是非负的。

根据半正定的定义,因为原矩阵**** ,所以对于任意 维数匹配的向量,都必定满足:

既然这个不等式对任意 的都成立,那我们不妨为巧妙地"特制"一个值。我们令:

将二次型中展开,得:

把这个特制的带入先三项,得:

  1. 第一项:
  2. 第二项:
  3. 第三项:

其中,第一项和第二项抵消,则剩下的结果是:

提取公因式:

因为原始矩阵**** ,所以左边永远成立。因此,等式右边必定有。既然对于任意 的,都成立,这就完美契合了半正定矩阵的数学定义。

结论:****是半正定的。

4.唯一的一个"隐患":必须可逆

在这个证明中,我们理所当然地使用了​。在纯数学层面上,如果****不可逆(即存在零空间),舒尔补是无法直接计算的(需要用伪逆)。

在实际的多传感器融合架构中,被边缘化的状态通常是最老帧的位姿和相关的速度、零偏)几乎总是受到充足的观测约束(比如它与局部地图有多个特征点匹配,且有 IMU 预积分约束)。这些充足的约束保证了局部的**** 是一个严格的正定矩阵(),因此求逆是绝对安全的。

相关推荐
7yewh2 天前
SLAM 三维空间刚体运动(2)
数据结构·人工智能·机器人·嵌入式·slam
longlongzihan4 天前
LeetCode 1004.最大连续1的个数III 滑动窗口及动态规划题解
c++·算法·leetcode·dp·滑动窗口
Logic10116 天前
C语言/数据结构滑动窗口题解:替换k个字符后的最长连续相同字符子串(LeetCode 424)
c语言·数据结构·字符串·滑动窗口·时间复杂度·频率统计·算法题
音视频牛哥17 天前
从 LLM、VLA、LLA、SLIM 到实时音视频感知底座:具身智能真正需要的不只是大模型
人工智能·llm·机器人视觉·slam·vla·多模态感知·机器人音视频
Tisfy19 天前
LeetCode 1477.找两个和为目标值且不重叠的子数组:滑动窗口 + 记录前值(动态规划)
动态规划·题解·dp·滑动窗口
a1879272183121 天前
【算法】双指针与滑动窗口(三):相向双指针——比较、排除、收缩
算法·leetcode·双指针·滑动窗口·原理·相向双指针·算法讲解
a1879272183122 天前
【算法】双指针与滑动窗口(二):滑动窗口——吃进、判定、吐出
算法·leetcode·双指针·滑动窗口·原理·模板·算法讲解
Evand J23 天前
【MATLAB例程,图像滤波降噪12】加权中值滑动窗口滤波(WMF)图像降噪,包含滤波质量评价。附代码下载链接
开发语言·图像处理·人工智能·计算机视觉·matlab·滑动窗口·滤波
Evand J23 天前
【MATLAB例程,图像降噪7】高斯加权滑动窗口滤波,用于图像降噪,带质量评价,附代码下载链接
开发语言·图像处理·matlab·滑动窗口·滤波·降噪·图像滤波
2601_9622935325 天前
第3.7章 机器人第三方库实战总结(七):OpenCV视觉SLAM核心库详解:API速查+源码实例+避坑指南
图像处理·opencv·计算机视觉·slam·第三方库