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

相关推荐
胡摩西1 天前
室内定位技术方法汇总:从WiFi到超声波,机器人如何在室内“找准自己”?
人工智能·机器人·slam·室内定位·roomaps
冰水不凉3 天前
amcl_pose vs tf的位姿输出频率
slam
巷尚UP3D-三维扫描检测逆向建模3 天前
Artec Jet:高精度测绘级SLAM激光扫描仪,自主高效【巷尚UP3D】
slam·高精度三维扫描·巷尚up3d
加油JIAX5 天前
LIO-SAM Scan-to-Map退化检测
slam·lio-sam·退化检测
加油JIAX5 天前
LIO-SAM系统架构
slam·imu·lio-sam
加油JIAX7 天前
滑动窗口模型与边缘化
slam·边缘化·滑动窗口优化
加油JIAX7 天前
IMU预积分-旋转残差求雅可比
slam·imu·预积分
CS_Zero8 天前
Faster-LIO论文与代码笔记(1)
笔记·slam·lio
加油JIAX10 天前
IMU预积分(VINS)
slam·imu·预积分·vins