边缘化求取先验约束

舒尔补定义

给定任意的矩阵块 M，如下所示：

$$M=\left[\begin{array}{cc}A& B\\ C& D\end{array}\right]$$

• 如果矩阵块 D 是可逆的，则 $A-B{D}^{-1}C$称之为 D 关于 M 的舒尔补，对此可以边缘化掉D对应的变量，保留A对应变量的约束关系。
• 如果矩阵块 A 是可逆的，则 $D-C{A}^{-1}B$称之为 A 关于 M 的舒尔补，对此可以边缘化掉A对应的变量，保留D对应变量的约束关系。

边缘化

随着环境探索的深入，系统状态变量（包括机器人的位姿和环境特征点的位置等）不断增加而导致的计算复杂度和内存消耗过高，这时候需要去除之前的变量，不再优化其参数，但其新的数据仍然存在约束，直接丢掉这些数据会造成约束信息丢失，所以要将其封装成先验信息，加入到大的非线性优化问题中作为一部分误差，这一步即为边缘化。

优化过程中会得到增量方程：
$$\begin{array}{rl}{J}^{T}J\delta x& =-J^{T}e\\ H\delta x& =b\end{array}$$
即：
$$\left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{12}^T& H_{22}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]$$

对上式进行舒尔补，两边同时左乘一个矩阵$\left[\begin{array}{cc}I& 0\\ -H_{12}^T{H}_{11}^{-1}& I\end{array}\right]$，得：

$$\left[\begin{array}{cc}{H}_{11}& H_{12}\\ 0& H_{22}-H_{12}^T{H}_{11}^{-1}{H}_{12}\end{array}\right]\left[\begin{array}{c}\delta x_1\\ \delta x_2\end{array}\right]=\left[\begin{array}{c}{b}_1\\ b_2\end{array}\right]$$

则：
$$(H_{22}-H_{12}^T{H}_{11}^{-1}{H}_{12})\delta x_2=b_2-H_{12}^T{H}_{11}^{-1}{b}_1$$
如此便把 $\delta x_1$ 给边缘化了，得到只关于 $\delta x_2$ 的公式，并最大化地利用了所有信息。

边缘化约束

令$H^{\prime }=H_{22}-H_{12}^T{H}_{11}^{-1}{H}_{12}$，$b^{\prime }=b_2-H_{12}^T{H}_{11}^{-1}{b}_1$，则
$$\begin{array}{rl}{H}^{\prime }\delta x_2& =b^{\prime }\end{array}$$

从 $H^{\prime }$、$b^{\prime }$中分解获取雅可比矩阵 $J$ 和残差 $e$：

因为$H^{\prime }$是实对称矩阵，可进行特征值分解得：
$$H^{\prime }=PD{P}^T$$
这里的 $P$ 是由特征向量组成的矩阵，而$D$ 是对角矩阵，包含特征值。

因为$D$ 是对角矩阵，所以易分解为：$D={\sqrt{D}}^T\sqrt{D}$，则
$$H^{\prime }=PD{P}^T=P{\sqrt{D}}^T\sqrt{D}{P}^T=(\sqrt{D}{P}^T{)}^T(\sqrt{D}{P}^T)$$
因为$H^{\prime }=J^{T}J$，所以
$$J=\sqrt{D}{P}^T$$

因为$b^{\prime }=-J^{T}e$，且$P^{-1}=P^T，{\sqrt{D}}^T=\sqrt{D}$，所以
$$e=-(J^T{)}^{-1}{b}^{\prime }=-((\sqrt{D}{P}^T{)}^T{)}^{-1}{b}^{\prime }=-((P{\sqrt{D}}^T){)}^{-1}{b}^{\prime }=-{\sqrt{D}}^{-1}{P}^T{b}^{\prime }$$

约束优化

记录边缘化得到的$J、e、\delta x_2$，这是优化过程中使用的变量。
在优化过程中，$\delta x_2$的值会改变，记为$\delta x_2^{\prime }$，而边缘化是约束$\delta x_2$尽量不变，因此
$$\delta e=\delta x_2^{\prime }-\delta x_2$$
其中，$\delta x_2$是边缘化是剩余的变量值，是固定的，整个优化过程不变

在这个优化过程中，雅可比矩阵 $J$ 保持不变，残差求取公式为：
$$\begin{array}{rl}{e}^{\prime }& =e+J\delta e\\ J^{\prime }& =J\end{array}$$
这里可以认为 $e^{\prime }=f(e)$，通过一阶泰勒展开后就有$e+J\delta e$。因为边缘化时求取了 $J$ ，认为在 $e$ 附近的导数为 $J$ ，这个值是保持不变的，由边缘化来固定这个值。换个说法，可以认为边缘化的目的是了解剩余变量在上一次优化后的值附近是如何变化的，即导数是什么。