从零真正理解SLAM中的边缘化---------详细的理论细节推导

1 边缘化是什么

回顾联合概率分布公式
$$P(x,y)=P(x|y)P(y)=P(y|x)P(x)$$

所谓边缘化就是从联合分布PDF计算其中一部分变量的PDF，如下面的公式描述
$$P(y)=\int P(x,y)dx=\int P(y|x)P(x)dx$$
通俗点说我们想边缘化掉x，那么只需要对这个联合分布中的x变量进行积分即可。

这个公式是理想情况下边缘化的做法，实际情况中我们是先构建了一个离散化的优化问题，直接在优化问题中操作边缘化，接下来我们来阐述如何从这个理想的公式到实际边缘化工程中的做法。

2 实际SLAM中的优化问题构建

2.1 线性高斯系统优化问题的构建（参考状态估计这本书，本节只是简介使得文章前后保持连续性）

求解一个优化问题在机器人状态估计中一般有两种方式，

• Bayesian Estimation：估计后验PDF的均值和方差
• Map A Posteriori Estimation：估计后验PDF的模及其方差

无论使用那种方式去推导，都可以导出滤波和优化两种具体的问题求解方法。我们使用MAP方式来导出，最后使用优化算法来求解问题。

⾸先，⽤贝叶斯公式重写 MAP 估计
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\max }p(\mathbf{x}\mid \mathbf{v},\mathbf{y})=\arg \underset{\mathbf{x}}{\max }\frac{p(\mathbf{y}\mid \mathbf{x},\mathbf{v})p(\mathbf{x}\mid \mathbf{v})}{p(\mathbf{y}\mid \mathbf{v})}=\arg \underset{\mathbf{x}}{\max }p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x}\mid \mathbf{v})$$

接下来假设：

• 运动噪声 ${\mathbf{w}}_k$ 和观测噪声 ${\mathbf{n}}_k$ 对于所有时刻都是彼此无关的

这样可以利用贝叶斯公式进行因子分解
$$p(\mathbf{y}\mid \mathbf{x})=\prod _{k=0}^{K}p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)$$
$$p(\mathbf{x}\mid \mathbf{v})=p\left({\mathbf{x}}_0\mid {\check{\mathbf{x}}}_0\right)\prod _{k=1}^{K}p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)$$

在线性高斯系统中，展开PDF。
$$\begin{array}{rl}p\left({\mathbf{x}}_0\mid {\check{\mathbf{x}}}_0\right)=& \frac{1}{\sqrt{(2\pi {)}^N\det {\check{\mathbf{P}}}_0}}\\ p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)=& \frac{1\exp \left(-\frac{1}{2}{\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_0^{-1}\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right)\right)}{\sqrt{(2\pi {)}^N\det {\mathbf{Q}}_k}}\\ p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)=& \frac{1}{\sqrt{(2\pi {)}^M\det {\mathbf{R}}_k}\left(-\frac{1}{2}{\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}_k^{-1}\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)\right)}\\ & \times \exp \left(-\frac{1}{2}{\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)\right)\end{array}$$

对两侧取负对数，下面是推导过程

$$\ln (p(\mathbf{y}\mid \mathbf{x})p(\mathbf{x}\mid \mathbf{v}))=\ln p\left({\mathbf{x}}_0\mid {\check{\mathbf{x}}}_0\right)+\sum _{k=1}^K\ln p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)+\sum _{k=0}^K\ln p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)$$

$$\begin{array}{rl}& \ln p\left({\mathbf{x}}_0\mid {\check{\mathbf{x}}}_0\right)=-\frac{1}{2}{\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_0^{-1}\left(\mathbf{x}-{\check{\mathbf{x}}}_0\right)\\ & -\underset{\text{与 }\mathbf{x}\text{ 无关 }}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^N\det {\check{\mathbf{P}}}_0\right)}}\\ & \ln p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)=-\frac{1}{2}{\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}_k^{-1}\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)\\ & -\underset{\text{与 }\mathbf{x}\text{ 无关 }}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^N\det \left({\mathbf{Q}}_k\right)\right)}}\\ & \ln p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)=-\frac{1}{2}{\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)\\ & -\underset{\text{与 }\mathbf{x}\text{ 无关 }}{\underbrace{\frac{1}{2}\ln \left((2\pi {)}^M\det {\mathbf{R}}_k\right)}}\\ & \end{array}$$

上面推导中出现了⼀些与 x ⽆关的项，直接忽略掉，因为不包含我们的优化变量。

定义下面的变量：
$$\begin{array}{rl}& J_{v,k}(\mathbf{x})=\{\begin{array}{ll}\frac{1}{2}{\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right)}^{\mathrm{T}}{\check{\mathbf{P}}}_0^{-1}\left({\mathbf{x}}_0-{\check{\mathbf{x}}}_0\right),& k=0\\ \frac{1}{2}{\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right)}^{\mathrm{T}}{\mathbf{Q}}_k^{-1}\left({\mathbf{x}}_k-{\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}-{\mathbf{v}}_k\right),& k=1\dots K\end{array}\\ & J_{y,k}(\mathbf{x})=\frac{1}{2}{\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right)}^{\mathrm{T}}{\mathbf{R}}_k^{-1}\left({\mathbf{y}}_k-{\mathbf{C}}_k{\mathbf{x}}_k\right),k=0\dots K\end{array}$$

这些都是马氏距离（Mahalanobis Distance），是个优雅的二次型形式。
定义我们的目标函数
$$J(\mathbf{x})=\sum _{k=0}^K\left(J_{v,k}(\mathbf{x})+J_{y,k}(\mathbf{x})\right)$$

问题的最优解为
$$\hat{\mathbf{x}}=\arg \underset{\mathbf{x}}{\min }J(\mathbf{x})$$

这个问题是⼀个无约束的优化问题（unconstrained optimization problem），对于状态变量 x 本⾝并没有任何约束

2.2 非线性非高斯系统（Non Linear Non Gaussian，NLNG）优化问题构建

考虑非线性系统的运动和观测模型
$$\begin{array}{rl}& {\mathbf{x}}_k=\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right),k=1\dots K\\ & {\mathbf{y}}_k=\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right),k=0\dots K\end{array}$$

构建NLNG系统的优化问题，考虑线性化方式（简单提一下：如果用滤波，你也可以用采样的方式，比如粒子滤波，sigma point kalman filter）：

$$\begin{array}{rl}& \begin{array}{rl}\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)& \approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{w}}_k^{\prime }\\ \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)& \approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{n}}_k^{\prime }\end{array}\\ & \begin{array}{c}{\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right),{\mathbf{F}}_{k-1}={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,\mathrm{C}}\\ {\mathbf{w}}_k^{\prime }={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}{\mathbf{w}}_k\end{array}\\ & \begin{array}{c}{\check{\mathbf{y}}}_k=\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right),{\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}\\ {\mathbf{n}}_k^{\prime }={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}{\mathbf{n}}_k\end{array}\end{array}$$

仿照线性高斯系统的MAP建模方式，NLNG系统优化问题建模如下
$$\mathbf{x}=\left[\begin{array}{c}{\mathbf{x}}_0\\ {\mathbf{x}}_1\\ ⋮\\ {\mathbf{x}}_K\end{array}\right]$$

定义残差
$$\begin{array}{rl}& {\mathbf{r}}_{v,k}(\mathbf{x})=\{\begin{array}{ll}{\check{\mathbf{x}}}_0-{\mathbf{x}}_0& k=0\\ \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,0\right)-{\mathbf{x}}_k& k=1\dots K\end{array}\\ & {\mathbf{r}}_{y,k}(\mathbf{x})={\mathbf{y}}_k-\mathbf{g}\left({\mathbf{x}}_k,0\right),k=0\dots K\end{array}$$

目标函数为
$$\begin{array}{rl}{J}_{v,k}(\mathbf{x})& =\frac{1}{2}{\mathbf{r}}_{v,k}(\mathbf{x}{)}^{\mathrm{T}}{\mathbf{W}}_{v,k}^{-1}{\mathbf{r}}_{v,k}(\mathbf{x})\\ J_{y,k}(\mathbf{x})& =\frac{1}{2}{\mathbf{r}}_{y,k}(\mathbf{x}{)}^{\mathrm{T}}{\mathbf{W}}_{y,k}^{-1}{\mathbf{r}}_{y,k}(\mathbf{x})\\ J(\mathbf{x})& =\sum _{k=0}^K\left(J_{v,k}(\mathbf{x})+J_{y,k}(\mathbf{x})\right)\end{array}$$

2.3 NLNG问题的Batch形式（实际SLAM优化多帧pose和多路标点的情况）

将上述目标函数写成矩阵形式
$$\begin{array}{c}\mathbf{r}(\mathbf{x})=\left[\begin{array}{c}{\mathbf{r}}_v(\mathbf{x})\\ {\mathbf{r}}_y(\mathbf{x})\end{array}\right],{\mathbf{r}}_v(\mathbf{x})=\left[\begin{array}{c}{\mathbf{r}}_{v,0}(\mathbf{x})\\ ⋮\\ {\mathbf{r}}_{v,K}(\mathbf{x})\end{array}\right],{\mathbf{r}}_y(\mathbf{x})=\left[\begin{array}{c}{\mathbf{r}}_{y,0}(\mathbf{x})\\ ⋮\\ {\mathbf{r}}_{y,K}(\mathbf{x})\end{array}\right]\\ \mathbf{W}=\mathrm{diag}\left({\mathbf{W}}_v,{\mathbf{W}}_y\right)\\ {\mathbf{W}}_v=\mathrm{diag}\left({\mathbf{W}}_{v,0},\dots ,{\mathbf{W}}_{v,K}\right),{\mathbf{W}}_y=\mathrm{diag}\left({\mathbf{W}}_{y,0},\dots ,{\mathbf{W}}_{y,K}\right)\end{array}$$

$$J(\mathbf{x})=\frac{1}{2}\mathbf{r}(\mathbf{x}{)}^{\mathrm{T}}{\mathbf{W}}^{-1}\mathbf{r}(\mathbf{x})$$

在实际问题求解中，我们会把噪声项和误差项进行合并写成

$$\mathbf{u}(\mathbf{x})=\mathbf{L}e(\mathbf{x})$$
$$J(\mathbf{x})=\frac{1}{2}\mathbf{u}(\mathbf{x}{)}^{\mathrm{T}}\mathbf{u}(\mathbf{x})$$

这可以用Cholesky分解得到
$${\mathbf{L}}^{\mathrm{T}}\mathbf{L}={\mathbf{W}}^{-1}$$

实际问题中我们都不需要用Cholesky分解，用这个W是一个对角阵，只需要针对对角线元素求sqrt即可。

2.4 使用高斯牛顿法求解

对 $u(x)$进行线性化（泰勒展开）
$$\mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}+\Delta \mathbf{x}\right)\approx \mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}\right)+\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)\Delta \mathbf{x}$$

将其代⼊目标函数 J 中
$$J\left({\mathbf{x}}_{\mathrm{op}}+\Delta \mathbf{x}\right)\approx \frac{1}{2}{\left(\mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}\right)+\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)\Delta \mathbf{x}\right)}^{\mathrm{T}}\left(\mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}\right)+\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)\Delta \mathbf{x}\right)$$

针对 ${\Delta }_x$ 最⼩化
$$\begin{array}{r}\frac{\partial J\left({\mathbf{x}}_{\mathrm{op}}+\Delta \mathbf{x}\right)}{\partial \Delta \mathbf{x}}={\left(\mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}\right)+\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)\Delta {\mathbf{x}}^{\ast }\right)}^{\mathrm{T}}\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)=0\\ \Rightarrow {\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)}^{\mathrm{T}}\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)\Delta {\mathbf{x}}^{\ast }=-{\left({\frac{\partial \mathbf{u}(\mathbf{x})}{\partial \mathbf{x}}|}_{{\mathbf{x}}_{\mathrm{op}}}\right)}^{\mathrm{T}}\mathbf{u}\left({\mathbf{x}}_{\mathrm{op}}\right)\end{array}$$

至此可以求解出 ${\Delta }_x$，然后 ${\hat{x}}_{k+1}={\hat{x}}_k+{\Delta }_x$，完成了一次迭代优化，持续迭代到 ${\Delta }_x$足够小完成优化问题求解。

2.5 考虑一个小的优化问题：仅包含观测模型

$$X^{MAP}=\underset{X}{\mathrm{argmin}}\sum _i{\Vert h_i\left(X_i\right)-z_i\Vert }_{{\Sigma }_i}^2$$
对观测模型进行线性化
$$h_i\left(X_i\right)=h_i\left(X_i^0+{\Delta }_i\right)\approx h_i\left(X_i^0\right)+J_i{\Delta }_i$$
$${J_i\triangleq \frac{\partial h_i\left(X_i\right)}{\partial X_i}|}_{X_i^0}$$
其中 ${\Delta }_i\triangleq X_i-X_i^0$ 就是我们的状态更新方程
带入上式

$$\begin{array}{rl}{\Delta }^{\ast }& =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert h_i\left(X_i^0\right)+J_i{\Delta }_i-z_i\Vert }_{{\Sigma }_i}^2\\ & =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert J_i{\Delta }_i-\left\{z_i-h_i\left(X_i^0\right)\right\}\Vert }_{{\Sigma }_i}^2\\ & =\underset{\Delta }{\mathrm{argmin}}\sum _i{\Vert J_i{\Delta }_i-r_i\Vert }_{{\Sigma }_i}^2\end{array}$$
其中 $J_i$ 称为Jacobian，
$r_i=\left\{z_i-h_i\left(X_i^0\right)\right\}$ 称为残差
我们把观测误差的协方差矩阵合并到 $J_i,r_i$中去，变成一个完美的二次型形式
$$\begin{array}{rl}{J}_i& ={\Sigma }_i^{-1/2}{J}_i\\ r_i& ={\Sigma }_i^{-1/2}\left(z_i-h_i\left(X_i^0\right)\right)\end{array}$$

上面的公式之所以成立，是因为
$$\Vert r{\Vert }_{\Sigma }^2\triangleq r^{\top }{\Sigma }^{-1}r={\left({\Sigma }^{-1/2}r\right)}^{\top }\left({\Sigma }^{-1/2}r\right)={\Vert {\Sigma }^{-1/2}r\Vert }_2^2$$

这里我们没有对jacobian和residual重新赋值一个新的名字，是为了方便。 因为实际工程中比如使用ceres构建factor的时候，会在factor evaluate函数中直接把信息矩阵（测量噪声）合并到jacobian和residual中。如下图

  residual = sqrt_info * residual;
  jacobian_pose_j.leftCols<6>() = sqrt_info * jaco_j;

所以最终的优化目标函数为
$$\begin{array}{rl}{\Delta }^{\ast }& =\underset{\Delta }{\mathrm{argmin}}\sum _i\Vert J_i{\Delta }_i-r_i\Vert \end{array}$$

3 边缘化（Marginalization）

3.1 边缘化想做什么事情

定义$X=\left[\begin{array}{c}{x}_m\\ x_n\end{array}\right]$

在滑动窗口算法中，经历完以次优化之后，我们希望把$x_m$从优化问题中移除，进而限制优化问题规模的增长，只保留 $x_n$。

如果直接把 $x_m$ 相关的变量和协方差直接移除，会导致 $x_m$对系统的约束直接就丢掉了，那么我们能不能丢掉 $x_m$的同时，保留 $x_m$对系统的约束呢？

需要先弄明白所谓对系统的约束是什么？

2.5 中我们考了一个简单的优化问题，其最终的目标函数为
$$\begin{array}{rl}{\Delta }^{\ast }& =\underset{\Delta }{\mathrm{argmin}}\sum _i\Vert J_i{\Delta }_i-r_i\Vert \end{array}$$
应用高斯牛顿方法，这个优化问题实际上是求解
$$J^{T}J\Delta =J^{T}r$$

其中 $J,r$是 $J_i,r_i$ 拼接成的大型稀疏矩阵

我们系统中不同变量之间的约束就是 $J^{T}J$ 这个矩阵中的那些不为0的元素。

令 $A=J{J}^T,b=J^{T}r$，可以写成
$$A\Delta =b$$

至此我们知道了我们希望边缘化做什么事情：

• 构建 $A\Delta =b$ 方程
• 将待marg的变量排列到矩阵A的最左侧，残差b的最上方
• 对矩阵A执行marg操作，同步对b进行修改，得到 $A_n{\Delta }_n=B_n$
• 从 $A_n,B_n$中恢复 $J_n,r_n$构建 $J_n^T{J}_n{\Delta }_n=J_n^T{r}_n$求解问题
• 进而有 $J_n,r_n$ 恢复子jacobian和子residual $J_{ni},r_{ni}$
• 最终得到边缘化之后的先验因子：${\Delta }^{\ast }=\underset{\Delta }{\mathrm{argmin}}{\sum }_i\Vert J_{ni}{\Delta }_{ni}-r_{ni}\Vert$

3.2 边缘化过程详细步骤（以VINS-MONO为例）

第一步：构建 $A\Delta =b$ 方程

在VINS-MONO中构建这个方程的过程就是 ThreadsConstructA 函数。
矩阵A 可视化如下

第二步：将待marg的变量排列到矩阵A的最左侧，残差b的最上方

$$\begin{gathered} A=\left[\begin{array}{cc}{\mathbf{\Sigma }}_{mm}& {\mathbf{\Sigma }}_{mn}\\ {\mathbf{\Sigma }}_{nm}& {\mathbf{\Sigma }}_{nn}\end{array}\right] \\ b=\left[\begin{array}{c}{\mathbf{b}}_{\mathbf{m}}\\ {\mathbf{b}}_{\mathbf{n}}\end{array}\right] \end{gathered}$$

第三步：对矩阵A执行marg操作，同步对b进行修改，得到 $A_n{\Delta }_n=B_n$

代码对应于marginalize()函数

marg操作，本质就是通过矩阵变换来消除掉 $x_m$ 变量，过程如下
对矩阵A执行LDU分解（同理如果想边缘化掉 $x_n$，就执行UDL分解），

UDL分解
$$\left[\begin{array}{cc}{\mathbf{\Sigma }}_{mm}& {\mathbf{\Sigma }}_{mn}\\ {\mathbf{\Sigma }}_{nm}& {\mathbf{\Sigma }}_{nn}\end{array}\right]=\left[\begin{array}{cc}1& {\mathbf{\Sigma }}_{mn}{\mathbf{\Sigma }}_{nn}^{-1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{\Sigma }}_{mm}-{\mathbf{\Sigma }}_{mn}{\mathbf{\Sigma }}_{nn}^{-1}{\mathbf{\Sigma }}_{nm}& 0\\ 0& {\mathbf{\Sigma }}_{nn}\end{array}\right]\left[\begin{array}{cc}1& 0\\ {\mathbf{\Sigma }}_{nn}^{-1}{\mathbf{\Sigma }}_{nm}& 1\end{array}\right]$$

LDU分解
$$\left[\begin{array}{cc}{\mathbf{\Sigma }}_{mm}& {\mathbf{\Sigma }}_{mn}\\ {\mathbf{\Sigma }}_{nm}& {\mathbf{\Sigma }}_{nn}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ {\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}& 1\end{array}\right]\left[\begin{array}{cc}{\mathbf{\Sigma }}_{mm}& 0\\ 0& {\mathbf{\Sigma }}_{nn}-{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}{\mathbf{\Sigma }}_{mn}\end{array}\right]\left[\begin{array}{cc}1& {\mathbf{\Sigma }}_{mm}^{-1}{\mathbf{\Sigma }}_{mn}\\ 0& 1\end{array}\right]$$

然后方程两边同时乘以
$$L^{-1}=\left[\begin{array}{cc}1& 0\\ -{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}& 1\end{array}\right]$$

最后化简

最终得到
$$\left({\mathbf{\Sigma }}_{nn}-{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}{\mathbf{\Sigma }}_{mn}\right){\Delta }_n=b_n-{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}{b}_m$$

那么 $$A_n=\left({\mathbf{\Sigma }}_{nn}-{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}{\mathbf{\Sigma }}_{mn}\right)$$
$$B_n=b_n-{\mathbf{\Sigma }}_{nm}{\mathbf{\Sigma }}_{mm}^{-1}{b}_m$$

第四步：从 $A_n,B_n$中恢复 $J_n,r_n$构建 $J_n^T{J}_n{\Delta }_n=J_n^T{r}_n$求解问题

• 最简单的可以对 $A_n$直接做cholesky分解， $$A_n=J_n^T{J}_n$$
• VINS MONO中使用了特征值分解
  $$\begin{gathered} A_n=VS{V}^T \\ =V{S}^{\frac{1}{2}}{S}^{\frac{1}{2}}{V}^T \\ =(S^{\frac{1}{2}}{V}^T{)}^T{S}^{\frac{1}{2}}{V}^T \end{gathered}$$

因此 $$J_n=S^{\frac{1}{2}}{V}^T$$
$$J_n^T=V{S}^{\frac{1}{2}}$$
而$J^T{r}_n=B_n$,所以
$$\begin{gathered} r_n={J^T}^{-1}{B}_n \\ =(V{S}^{\frac{1}{2}}{)}^{-1}{B}_n \\ =S^{-\frac{1}{2}}{V}^{-1}{B}_n \\ =S^{-\frac{1}{2}}{V}^T{B}_n \end{gathered}$$

第五步：进而有 $J_n,r_n$ 恢复子jacobian和子residual $J_{ni},r_{ni}$

这一步实际中并没有拆出来，而是直接在第六步中批量计算了，因为所有的优化子factor最后还是汇聚成一个大矩阵，现在我们已经有了这个矩阵，自然不需要再重复操作了。
这个主要是展示构建先验项每一个小factor的组成

第六步：最终得到边缘化之后的先验因子：${\Delta }^{\ast }=\underset{\Delta }{\mathrm{argmin}}{\sum }_i\Vert J_{ni}{\Delta }_{ni}-r_{ni}\Vert$

bool MarginalizationFactor::Evaluate(double const *const *parameters,
                                     double *residuals,
                                     double **jacobians) const {

• 注意Evaluate中函数的jacobian项就是 $J_{ni}$，
• 残差的计算为
  $$\text{residual}=J_n\left(X-X_0\right)-r_n$$

这个 VINS-MONO中最后实现的不同，VINS-MONO是 $+r_n$，原因是vins-mono中定义的
$$r=h(x)-z$$

本文定义残差为
$$r_i=\left\{z_i-h_i\left(X_i^0\right)\right\}$$

二者是相反的，所以符号不同。

Reference

• 机器人学中状态估计
• 机器人感知：因子图在SLAM中的应用
• VINS论文推导及代码解析_V13_190317.pdf，崔华坤