为什么边缘化先验的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 预积分约束)。这些充足的约束保证了局部的**** 是一个严格的正定矩阵(,因此求逆是绝对安全的。

相关推荐
bing_feilong3 天前
FAST_LIO2(2): 运行记录
机器学习·slam
j7~4 天前
【算法】专题二:滑动窗口之水果成蓝,找到字符串中所有字⺟异位词等算法题
c++·算法·滑动窗口·水果成蓝·最小字串覆盖·优选算法精选
大江东去浪淘尽千古风流人物4 天前
【SLAM】slam高动态位姿估计全链路拆解
算法·面试·职场和发展·视觉里程计·slam·vio·大疆
Agilex松灵机器人5 天前
UMR × NAVIS:移动机器人行业级通用一体化底盘与3D导航解决方案
人工智能·3d·机器人·ros·slam·vla·松灵机器人
a1117767 天前
SLAM 如何一步步迭代到 SE(3)
slam
a1117767 天前
点云拼接 (RGB-D 点云建图)
算法·slam
辰尘_星启8 天前
【三维重建】概述
计算机视觉·机器人·slam·控制·三维重建·sfm
闪电悠米8 天前
力扣hot100-438.找到字符串中所有字母异位词-固定长度滑动窗口详解
linux·服务器·数据结构·算法·leetcode·滑动窗口·力扣hot100
chase_my_dream9 天前
FAST-LIO src/IMU_Processing.hpp 完整详细讲解
c++·状态模式·slam
Tisfy11 天前
LeetCode 1358.包含所有三种字符的子字符串数目:滑动窗口(两种写法直接推荐方法二)
算法·leetcode·字符串·题解·滑动窗口