PDF 视觉SLAM十四讲从理论到实践 第2版

前6讲（数学基础）知识笔记见 Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Mathematical basis)

7-8讲（视觉里程计）知识笔记见 Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Practical application P1)

11-12讲（回环检测 & 建图）知识笔记见 Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Practical application P3)

第二部分 实践应用

第9讲 后端 1

前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，这个地图在长时间内是不准确的。

在视觉里程计的基础上，还希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。

概述

状态估计的概率解释

第2讲中提到，视觉里程计只有短暂的记忆，而我们希望整个运动轨迹在较长时间内都能保持最优的状态。

在后端优化中，通常考虑一段更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新，这种处理方式称为“批量的”（Batch）

否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，则称为“渐进的”（Incremental）

已经知道SLAM过程可以由运动方程和观测方程来描述。

那么，假设在 $t=0$ 到 $t=N$ 的时间内，有位姿 $\boldsymbol{x}_{0}$ 到 $\boldsymbol{x}_{N}$，并且有路标 $\boldsymbol{y}_{1},\ldots,\boldsymbol{y}_{M}$

按照之前的写法，运动和观测方程为：

$$\left\{\begin{array}{l} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k, j} \end{array} \quad k=1, \ldots, N, j=1, \ldots, M .\right.$$ 观测方程中，只有当 $\boldsymbol{x}_{k}$ 看到了 $\boldsymbol{y}_{j}$ 时，才会产生观测数据，否则就没有。事实上，在一个位置通常只能看到一小部分路标。而且，由于视觉SLAM特征点数量众多、所以实际中观测方程的数量会远远大于运动方程。

可能没有测量运动的装置，也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。

在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这就非常类似于SfM问题，相当于通过一组图像来恢复运动和结构。不同的是，SLAM 中的图像有时间上的先后顺序，而SfM中允许使用完全无关的图像。

每个方程都受噪声影响，所以要把这里的位姿 $\boldsymbol{x}$ 和路标 $\boldsymbol{y}$ 看成服从某种概率分布的随机变量，而不是单独的一个数。

因此，问题就变成：当拥有某些运动数据 $\boldsymbol{u}$ 和观测数据 $\boldsymbol{z}$ 时，如何确定状态量 $\boldsymbol{x}$，$\boldsymbol{y}$ 的分布？

在比较常见且合理的情况下，假设状态量和噪声项服从高斯分布——这意味着在程序中只需要存储它们的均值和协方差矩阵即可。均值可看作对变量最优值的估计，而协方差矩阵则度量了它的不确定性。

那么，问题就转变为：当存在一些运动数据和观测数据时，如何估计状态量的高斯分布？

只有运动方程时，相当于蒙着眼睛在一个未知的地方走路。尽管知道自己每一步走了多远，但是随着时间流逝，越来越不确定自己的位置——内心也就越不安。这说明当输入数据受噪声影响时，误差是逐渐累积的，对位置方差的估计将越来越大。的估计将越来越大。但是，当我们睁开眼睛时，由于能够不断地观测到外部场景，使得位置估计的不确定性变小，我们就会越来越自信。

上面的过程以比喻的形式解释了状态估计中的问题，下面要以定量的方式来看待它。

在第6讲中，介绍了最大似然估计，提到批量状态估计问题可以转化为最大似然估计问题，并使用最小二乘法进行求解。

如何将该结论应用于渐进式问题，得到一些经典的结论。

同时，在视觉SLAM里，最小二乘法又有何特殊的结构。

首先，由于位姿和路标点都是待估计的变量，令 $\boldsymbol{x}_{k}$ 为 $k$ 时刻的所有未知量。它包含了当前时刻的相机位姿与 $m$ 个路标点，有：

$$\boldsymbol{x}_{k} \stackrel{\text { def }}{=}\left\{\boldsymbol{x}_{k}, \boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{m}\right\}$$

同时，把 $k$ 时刻的所有观测记作 $\boldsymbol{z}_{k}$

于是，运动方程与观测方程的形式可写得更简洁：

$$\left\{\begin{array}{l} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k} \end{array} \quad k=1, \ldots, N .\right.$$

考虑第 $k$ 时刻的情况

用过去 $0$ 到 $k$ 中的数据来估计现在的状态分布：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right)$$

下标 $0:k$ 表示从 $0$ 时刻到 $k$ 时刻的所有数据。

按照贝叶斯法则，把 $\boldsymbol{z}_{k}$ 与 $\boldsymbol{z}_{x}$ 交换位置，有：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right) \propto P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)$$

第一项称为似然，第二项称为先验。

似然由观测方程给定

而先验部分，当前状态 $\boldsymbol{x}_{k}$ 是基于过去所有的状态估计得来的，至少会受 $\boldsymbol{x}_{k-1}$ 影响，以 $\boldsymbol{x}_{k-1}$ 时刻为条件概率展开：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=\int P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}$$

如果考虑更久之前的状态，也可以继续对此式进行展开，但现在只关心 $k$ 时刻和 $k-1$ 时刻的情况。

至此，给出了贝叶斯估计，因为上式还没有具体的概率分布形式，所以没法实际操作它。

对这一步的后续处理方法有两种：

1. 假设马尔可夫性，简单的一阶马氏性认为，$k$ 时刻状态只与 $k-1$ 时刻状态有关，而与再之前的无关。如果做出这样的假设，就会得到以扩展卡尔曼滤波（EKF） 为代表的滤波器方法。在滤波方法中，会从某时刻的状态估计，推导到下一个时刻。
2. 考虑 $k$ 时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。目前，视觉SLAM的主流为非线性优化方法。

线性系统和KF

当假设了马尔可夫性，当前时刻状态只与上一个时刻有关，式 $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=\int P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}$ 中右侧第一部分可简化为：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)$$

这里由于 $k$ 时刻状态与 $k-1$ 之前的无关，所以就简化成只与 $\boldsymbol{x}_{k-1}$ 和 $\boldsymbol{u}_{k}$ 有关的形式，与 $k$ 时刻的运动方程对应。

第二部分可简化为：

$$P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k-1}, \boldsymbol{z}_{1: k-1}\right) .$$

考虑到 $k$ 时刻的输入量 $\boldsymbol{u}_{k}$ 与 $k-1$ 时刻的状态无关，所以将 $\boldsymbol{u}_{k}$ 拿掉，这一项实际上是 $k-1$ 时刻的状态分布。

于是，这一系列方程说明，实际在做的是“如何把 $k-1$ 时刻的状态分布推导至 $k$ 时刻”这样一件事。

也就是说，在程序运行期间，只要维护一个状态量，对它不断地进行迭代和更新即可。

如果假设状态量服从高斯分布，那么只需考虑维护状态量的均值和协方差即可。

从形式最简单的线性高斯系统开始，最后得到卡尔曼滤波器。

线性高斯系统是指，运动方程和观测方程可以由线性方程来描述：

$$\left\{\begin{array}{l} \boldsymbol x_{k}=\boldsymbol{A}_{k} \boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=\boldsymbol{C}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k} \end{array} \quad k=1, \ldots, N\right.$$

并假设所有的状态和噪声均满足高斯分布，记噪声服从零均值高斯分布：

$$\boldsymbol{w}_{k} \sim N(\mathbf{0}, \boldsymbol{R}) . \quad \boldsymbol{v}_{k} \sim N(\mathbf{0}, \boldsymbol{Q})$$

利用马尔可夫性，假设知道了 $k-1$ 时刻的后验（在 $k-1$ 时刻看来）状态估计 $\hat{\boldsymbol x}_{k-1}$ 及其协方差 $\hat{\boldsymbol P}_{k-1}$，现在要根据 $k$ 时刻的输入和观测数据，确定 $\boldsymbol x_k$ 的后验分布。

以上帽子 $\hat {\boldsymbol x}_k$ 表示后验，以下帽子 $\check{\boldsymbol x}_{k}$ 表示先验分布。

卡尔曼滤波器的第一步，通过运动方程确定 $\boldsymbol x_k$ 的先验分布。这一步是线性的，而高斯分布的线性变换仍是高斯分布。所以显然有：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=N\left(\boldsymbol{A}_{k} \hat{\boldsymbol x}_{k-1}+\boldsymbol{u}_{k}, \boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R}\right) .$$

这一步称为预测（Predict），它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声）推断当前时刻的状态分布。这个分布也就是先验。记：

$$\check{\boldsymbol x}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol u_{k}, \quad \check{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R} .$$

一方面，显然这一步状态的不确定度要变大，因为系统中添加了噪声。

另一方面，由观测方程，可以计算在某个状态下应该产生怎样的观测数据：

$$P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right)=N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right) .$$

为了得到后验概率，想要计算它们的乘积，也就是由式 $P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right) \propto P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)$ 给出的贝叶斯公式。

先把结果设为 $\boldsymbol{x}_{k} \sim N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)$，那么：

$$N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)=\eta N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right) \cdot N\left(\check{\boldsymbol{x}}_{k}, \check{\boldsymbol{P}}_{k}\right)$$

这里既然已经知道等式两侧都是高斯分布，那就只需比较指数部分，无须理会高斯分布前面的因子部分。

指数部分很像是一个二次型的配方。首先把指数部分展开，有：

$$\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)^{\mathrm{T}} \hat{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)=\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)^{\mathrm{T}} \boldsymbol{Q}^{-1}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)+\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)^{\mathrm{T}} \check{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)$$

为了求左侧的 $\hat{\boldsymbol{x}}_{k}$ 和 $\hat{\boldsymbol{P}}_{k}$，将两边展开，并比较 $\boldsymbol {x}_{k}$ 的二次和一次系数。

对于二次系数，有：

$$\hat{\boldsymbol{P}}_{k}^{-1}=\boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k}+\check{\boldsymbol{P}}_{k}^{-1}$$

该式给出协方差的计算过程，在式左右各乘 $\hat{\boldsymbol{P}}_{k}$，有：

$$\boldsymbol{I}=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k}+\hat{\boldsymbol{P}}_{k} \check{\boldsymbol{P}}_{k}^{-1}$$

令 $\boldsymbol K=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1}$，于是有：

$$\hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \check{\boldsymbol{P}}_{k} .$$

这里看似有一点儿循环定义的意思。我们由 $\hat{\boldsymbol{P}}_{k}$ 定义了 $\boldsymbol K$，又把 $\hat{\boldsymbol{P}}_{k}$ 写成了 $\boldsymbol K$ 的表达式。

然而，实际中 $\boldsymbol K$ 可以不依靠 $\hat{\boldsymbol{P}}_{k}$ 算得，但是这需要引入SMW（Sherman-Morrison-Woodbury）恒等式。

然后比较一次项系数，有：

$$-2 \hat{\boldsymbol{x}}_{k}^{\mathrm{T}} \hat{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k}=-2 \boldsymbol{z}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k} \boldsymbol{x}_{k}-2 \check{\boldsymbol{x}}_{k}^{\mathrm{T}} \check{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k}$$

整理（取系数并转置）得：

$$\hat{\boldsymbol{P}}_{k}^{-1} \hat{\boldsymbol{x}}_{k}=C_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol z_{k}+\check{P}_{k}^{-1} \check {\boldsymbol{x}}_{k}$$

两侧乘以 $\hat{\boldsymbol{P}}_{k}$ 并代入 $\boldsymbol K$，得：

$$\begin{aligned} \hat{\boldsymbol{x}}_{k} &=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{z}_{k}+\hat{\boldsymbol{P}}_{k} \check{\boldsymbol{P}}_{k}^{-1} \check{\boldsymbol{x}}_{k} \\ &=\boldsymbol{K} \boldsymbol{z}_{k}+\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \check{\boldsymbol{x}}_{k}=\check{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \check{\boldsymbol{x}}_{k}\right) . \end{aligned}$$

于是又得到了后验均值的表达。

总而言之，上面的两个步骤可以归纳为“预测”和“更新”（Update）两个步骤：

1. 预测：

   $$\check{\boldsymbol{x}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \quad \check{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R} .$$

2. 更新：先计算 $\boldsymbol K$，它又称为卡尔曼增益。

   $$\boldsymbol{K}=\check{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}}\left(\boldsymbol{C}_{k} \check{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}}+\boldsymbol{Q}_{k}\right)^{-1}$$

   然后计算后验概率的分布。

   $$\begin{array}{l} \hat{\boldsymbol{x}}_{k}=\check{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \check{\boldsymbol{x}}_{k}\right) \\ \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \check{\boldsymbol{P}}_{k} . \end{array}$$

至此，推导了经典的卡尔曼滤波器的整个过程。

事实上，卡尔曼滤波器有若干种推导方式，而我们使用的是从概率角度出发的最大后验概率估计的方式。

在线性高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。

而且，由于高斯分布经过线性变换后仍服从高斯分布，所以整个过程中没有进行任何的近似。

可以说，卡尔曼滤波器构成了线性系统的最优无偏估计。

非线性系统和EKF

SLAM中的运动方程和观测方程通常是非线性函数，尤其是视觉SLAM中的相机模型，需要使用相机内参模型及李代数表示的位姿，更不可能是一个线性系统。

一个高斯分布，经过非线性变换后，往往不再是高斯分布，所以在非线性系统中，必须取一定的近似，将一个非高斯分布近似成高斯分布。

把卡尔曼滤波器的结果拓展到非线性系统中，称为扩展卡尔曼滤波器。

通常的做法是，在某个点附近考虑运动方程及观测方程的一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。

令 $k-1$ 时刻的均值与协方差矩阵为 $\hat{\boldsymbol{x}}_{k-1},\hat{\boldsymbol{P}}_{k-1}$

在 $k$ 时刻，把运动方程和观测方程在 $\hat{\boldsymbol{x}}_{k-1},\hat{\boldsymbol{P}}_{k-1}$ 处进行线性化（相当于一阶泰勒展开），有：

$$\boldsymbol{x}_{k} \approx f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right)+\left.\frac{\partial f}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)+\boldsymbol{w}_{k} .$$

记这里的偏导数为：

$$\boldsymbol{F}=\left.\frac{\partial f}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}} .$$

同样对于观测方程，亦有：

$$\boldsymbol{z}_{k} \approx h\left(\check{\boldsymbol{x}}_{k}\right)+\left.\frac{\partial h}{\partial \boldsymbol{x}_{k}}\right|_{\check{\boldsymbol{x}}_{k}}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)+\boldsymbol{n}_{k}$$

记这里的偏导数为：

$$\boldsymbol{H}=\left.\frac{\partial h}{\partial \boldsymbol{x}_{k}}\right|_{\check{\boldsymbol{x}}_{k}} .$$

在预测步骤中，根据运动方程有：

$$P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol z_{0: k-1}\right)=N\left(f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right), \boldsymbol{F} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)$$

记这里的先验和协方差的均值为：

$$\check{\boldsymbol{x}}_{k}=f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right), \quad \check{\boldsymbol{P}}_{k}=\boldsymbol{F} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}^{\mathrm{T}}+\boldsymbol{R}_{k} .$$

然后考虑在观测中有：

$$P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right)=N\left(h\left(\check{\boldsymbol{x}}_{k}\right)+\boldsymbol{H}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right), \boldsymbol{Q}_{k}\right) .$$

最后，根据最开始的贝叶斯展开式，可以推导出 $\boldsymbol{x}_{k}$ 的后验概率形式。

先定义一个卡尔曼增益 $\boldsymbol{K}_{k}$：

$$\boldsymbol{K}_{k}=\check{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}\left(\boldsymbol{H} \check{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{Q}_{k}\right)^{-1} .$$

在卡尔曼增益的基础上，后验概率的形式为：

$$\hat{\boldsymbol{x}}_{k}=\check{\boldsymbol{x}}_{k}+\boldsymbol{K}_{k}\left(\boldsymbol{z}_{k}-h\left(\check{\boldsymbol{x}}_{k}\right)\right), \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}\right) \check{\boldsymbol{P}}_{k}$$

卡尔曼滤波器给出了在线性化之后状态变量分布的变化过程。

在线性系统和高斯噪声下，卡尔曼滤波器给出了无偏最优估计。

而在SLAM这种非线性的情况下，它给出了单次线性近似下的最大后验估计。

EKF的讨论

EKF 以形式简洁、应用广泛著称。

在早期的SLAM中，EKF 占据了很长一段时间的主导地位，时至今日，尽管认识到非线性优化比滤波器占有明显的优势，但是在计算资源受限，或待估计量比较简单的场合，EKF 仍不失为一种有效的方式。

EKF 的局限：

1. 滤波器方法在一定程度上假设了马尔可夫性，也就是 $k$ 时刻的状态只与 $k-1$ 时刻相关，而与 $k-1$ 之前的状态和观测都无关（或者和前几个有限时刻的状态相关）。

   这有点像是在视觉里程计中只考虑相邻两帧的关系。如果当前帧确实与很久之前的数据有关（例如回环），那么滤波器会难以处理。

   而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系，更会把很久之前的状态也考虑进来，称为全体时间上的SLAM（Full-SLAM）。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。

2. 与第6讲介绍的优化方法相比，EKF 滤波器仅在 $\hat{\boldsymbol{x}}_{k-1}$​ 处做了一次线性化，就直接根据这次线性化的结果，把后验概率给算了出来。

   这相当于认为该点处的线性化近似在后验概率处仍然是有效的。

   而实际上，当离工作点较远时，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性，那么线性近似就只在很小范围内成立，不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差，也是它的主要问题所在。

   在优化问题中，一阶（最速下降）或二阶（高斯牛顿法或列文伯格—马夸尔特方法）的近似，每迭代一次，状态估计发生改变之后，会重新对新的估计点做泰勒展开，而不像 EKF 那样只在固定点上做一次泰勒展开。这就使得优化的方法适用范围更广，在状态变化较大时也能适用。

   所以大体来说，可以粗略地认为 EKF 仅是优化中的一次迭代。

3. 从程序实现上来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。如果把路标也放进状态，由于视觉SLAM 中路标数量很大，则这个存储量是相当可观的，且与状态量呈平方增长（因为要存储协方差矩阵）。

   因此，普遍认为 EKF SLAM 不适用于大型场景。

4. EKF 等滤波器方法没有异常检测机制，导致系统在存在异常值的时候很容易发散。而在视觉SLAM 中，异常值却是很常见的：无论特征匹配还是光流法，都容易追踪或匹配到错误的点。没有异常值检测机制会让系统在实用中非常不稳定。

由于 EKF 存在这些明显的缺点，通常认为，在同等计算量的情况下，非线性优化能取得更好（精度和鲁棒性同时达到更好）的效果。

BA与图优化

所谓的 Bundle Adjustment（BA），是指从视觉图像中提炼出最优的3D模型和相机参数（内参数和外参数）。

考虑从任意特征点发射出来的几束光线（bundles of light rays），它们会在几个相机的成像平面上变成像素或是检测到的特征点。

如果调整（adjustment）各相机姿态和各特征点的空间位置，使得这些光线最终收束到相机的光心，就称为BA。

投影模型和BA代价函数

投影的过程，从一个世界坐标系中的点 $\boldsymbol p$ 出发，考虑相机的内外参数，最后投影成像素坐标。具体步骤如下：

1. 把世界坐标转换到相机坐标，这里将用到相机外参数 $(\boldsymbol R,\boldsymbol t)$：

   $$\boldsymbol{P}^{\prime}=\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t}=\left[X^{\prime}, Y^{\prime}, Z^{\prime}\right]^{\mathrm{T}}$$

2. 将 $\boldsymbol{P}^{\prime}$ 投至归一化平面，得到归一化坐标：

   $$\boldsymbol{P}_{\mathrm{c}}=\left[u_{\mathrm{c}}, v_{\mathrm{c}}, 1\right]^{\mathrm{T}}=\left[X^{\prime} / Z^{\prime}, Y^{\prime} / Z^{\prime}, 1\right]^{\mathrm{T}} .$$

3. 考虑归一化坐标的畸变情况，得到去畸变前的原始像素坐标，暂时只考虑径向畸变：

   $$\left\{\begin{array}{l} u_{\mathrm{c}}^{\prime}=u_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \\ v_{\mathrm{c}}^{\prime}=v_{\mathrm{c}}\left(1+k_{1} r_{\mathrm{c}}^{2}+k_{2} r_{\mathrm{c}}^{4}\right) \end{array}\right.$$

4. 根据内参模型，计算像素坐标：

   $$\left\{\begin{array}{l} u_{s}=f_{x} u_{\mathrm{c}}^{\prime}+c_{x} \\ v_{s}=f_{y} v_{\mathrm{c}}^{\prime}+c_{y} \end{array}\right.$$

流程图如下：

左侧的 $\boldsymbol p$ 是全局坐标系下的三维坐标点，右侧的 $u_{s},v_{s}$ 是该点在图像平面上的最终像素坐标。中间畸变模块中的 $r_c^2=u_c^2+v_c^2$

这个过程也是之前讲的观测方程，之前抽象地记成：

$$\boldsymbol z=h(\boldsymbol x,\boldsymbol y)$$

现在，给出了它的详细参数化过程。

具体地说，这里的 $\boldsymbol x$ 指代此时相机的位姿，即外参 $\boldsymbol R,\boldsymbol t$，它对应的李群为 $\boldsymbol T$，李代数为 $\boldsymbol \xi$。路标 $\boldsymbol y$ 即这里的三维点 $\boldsymbol p$，而观测数据则是像素坐标 $\boldsymbol{z} \stackrel{\text { def }}{=}\left[u_{s}, v_{s}\right]^{\mathrm{T}}$。

以最小二乘的角度来考虑，那么可以列写关于此次观测的误差：

$$e=\boldsymbol z-h(\boldsymbol T,\boldsymbol p)$$

然后，把其他时刻的观测量也考虑进来，给误差添加一个下标。

设 $\boldsymbol z_{ij}$ 为在位姿 $\boldsymbol T_i$ 处观察路标 $\boldsymbol p_i$ 产生的数据，那么整体的**代价函数**为：

$$\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{z}_{i j}-h\left(\boldsymbol{T}_{i}, \boldsymbol{p}_{j}\right)\right\|^{2}$$

对这个最小二乘进行求解，相当于对位姿和路标同时做了调整，也就是所谓的BA。

通过该目标函数和第6讲介绍的非线性优化内容，进行该模型的求解。

BA的求解

根据非线性优化的思想，从某个初始值开始，寻找下降方向 $\Delta \boldsymbol{x}$，来找到目标函数的最优解，即不断求解增量方程 $\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ 中的增量 $\Delta \boldsymbol{x}$。

尽管误差项都是针对单个位姿和路标点的，但在整体BA目标函数上，应把自变量定义为所有待优化的变量：

$$\boldsymbol{x}=\left[\boldsymbol{T}_{1}, \ldots, \boldsymbol{T}_{m}, \boldsymbol{p}_{1}, \ldots, \boldsymbol{p}_{n}\right]^{\mathrm{T}}$$

相应地，增量方程中的 $\Delta \boldsymbol{x}$ 则是对整体自变量的增量，在这个意义下，给自变量一个增量时，目标函数变为：

$$\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}+\boldsymbol{F}_{i j} \Delta \boldsymbol{\xi}_{i}+\boldsymbol{E}_{i j} \Delta \boldsymbol{p}_{j}\right\|^{2}$$

其中，$\boldsymbol{F}_{i j}$ 表示整个代价函数在当前状态下对相机姿态的偏导数，而 $\boldsymbol{E}_{i j}$ 表示该函数对路标点位置的偏导。

在第7讲最小化重投影误差求解PnP中介绍了它们的具体形式，所以这里就不再展开推导了。

现在，把相机位姿变量放在一起：

$$\boldsymbol{x}_{\mathrm{c}}=\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}, \ldots, \boldsymbol{\xi}_{m}\right]^{\mathrm{T}} \in \mathbb{R}^{6 m}$$

并把空间点的变量也放在一起：

$$\boldsymbol{x}_{p}=\left[\boldsymbol{p}_{1}, \boldsymbol{p}_{2}, \ldots, \boldsymbol{p}_{n}\right]^{\mathrm{T}} \in \mathbb{R}^{3 n}$$

那么，式 $\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2} \approx \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}+\boldsymbol{F}_{i j} \Delta \boldsymbol{\xi}_{i}+\boldsymbol{E}_{i j} \Delta \boldsymbol{p}_{j}\right\|^{2}$ 可简化为：

$$\frac{1}{2}\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2}=\frac{1}{2}\left\|\boldsymbol{e}+\boldsymbol{F} \Delta \boldsymbol{x}_{c}+\boldsymbol{E} \Delta \boldsymbol{x}_{p}\right\|^{2} .$$

需要注意的是，该式从一个由很多个小型二次项之和，变成矩阵形式。

这里的雅可比矩阵 $\boldsymbol{E}$ 和 $\boldsymbol{F}$ 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数 $\boldsymbol{F}_{ij}$ 和 $\boldsymbol{E}_{ij}$ 拼凑起来。

无论使用高斯牛顿法还是列文伯格—马夸尔特方法，最后都将面对增量线性方程：$\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$

根据第6讲的知识，高斯牛顿法和列文伯格—马夸尔特方法的主要差别在于，这里的 $\boldsymbol H$ 是取 $\boldsymbol J^T\boldsymbol J$ 还是 $\boldsymbol J^T\boldsymbol J+\lambda \boldsymbol I$ 的形式。

由于把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为：

$$\boldsymbol{J}=[\boldsymbol{F} \boldsymbol{E}]$$

以高斯牛顿法为例，则 $\boldsymbol H$ 矩阵为：

$$\boldsymbol{H}=\boldsymbol{J}^{\mathrm{T}} \boldsymbol{J}=\left[\begin{array}{ll} \boldsymbol{F}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{F}^{\mathrm{T}} \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{E}^{\mathrm{T}} \boldsymbol{E} \end{array}\right]$$

在列文伯格—马夸尔特方法中也需要计算这个矩阵。

不难发现，因为考虑了所有的优化变量，所以这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。

尤其是在视觉SLAM 中，一幅图像会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对 $\boldsymbol H$ 求逆来计算增量方程，由于矩阵求逆是复杂度为 $O(n^3)$ 的操作，那么消耗的计算资源会非常多。

幸运的是，这里的 $\boldsymbol H$ 矩阵是有一定的特殊结构的。利用这个特殊结构，可以加速求解过程。

稀疏性和边缘化

$\boldsymbol H$ 矩阵的稀疏性是由雅可比矩阵 $\boldsymbol J(\boldsymbol x)$ 引起的。

考虑这些代价函数当中的其中一个 $\boldsymbol e_{ij}$

注意，这个误差项只描述了在 $\boldsymbol T_i$ 看到 $\boldsymbol p_j$ 这件事，只涉及第 $i$ 个相机位姿和第 $j$ 个路标点，对其余部分的变量的导数都为 $0$。所以该误差项对应的雅可比矩阵有下面的形式：

$$\boldsymbol{J}_{i j}(\boldsymbol{x})=\left(\mathbf{0}_{2 \times 6}, \ldots \boldsymbol{0}_{2 \times 6}, \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{T}_{i}}, \boldsymbol{0}_{2 \times 6}, \ldots \boldsymbol{0}_{2 \times 3}, \ldots \boldsymbol{0}_{2 \times 3}, \frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{p}_{j}}, \boldsymbol{0}_{2 \times 3}, \ldots \boldsymbol{0}_{2 \times 3}\right)$$

其中 $\mathbf{0}_{2 \times 6}$ 表示维度为 $2×6$ 的 $\mathbf 0$ 矩阵，同理，$\mathbf{0}_{2 \times 3}$ 也是一样的。

该误差项对相机姿态的偏导 $\partial \boldsymbol{e}_{i j}/\partial \boldsymbol \xi_i$ 维度为 $2×6$，对路标点的偏导 $\partial \boldsymbol{e}_{i j}/\partial \boldsymbol p_i$ 维度是 $2×3$。

这个误差项的雅可比矩阵，除了这两处为非零块，其余地方都为零。

这体现了该误差项与其他路标和轨迹无关的特性。

从图优化角度来说，这条观测边只和两个顶点有关。

以下图为例，设 $\boldsymbol J_{ij}$ 只在 $i,j$ 处有非零块，那么它对 $\boldsymbol H$ 的贡献为 $\boldsymbol J^T_{ij}\boldsymbol J_{ij}$，具有图上所画的稀疏形式。

当某个误差项 $\boldsymbol J$ 具有稀疏性时，它对 $\boldsymbol H$ 的贡献也具有稀疏形式。

这个 $\boldsymbol J^T_{ij}\boldsymbol J_{ij}$ 矩阵也仅有4个非零块，位于 $(i,i),(i,j),(j,i),(j,j)$。

对于整体的 $\boldsymbol H$，有：

$$\boldsymbol{H}=\sum_{i, j} \boldsymbol{J}_{i j}^{\mathrm{T}} \boldsymbol{J}_{i j}$$

$i$ 在所有相机位姿中取值，$j$ 在所有路标点中取值。把 $\boldsymbol H$ 进行分块：

$$\boldsymbol{H}=\left[\begin{array}{ll} \boldsymbol{H}_{11} & \boldsymbol{H}_{12} \\ \boldsymbol{H}_{21} & \boldsymbol{H}_{22} \end{array}\right]$$

这里，$\boldsymbol H_{11}$ 只和相机位姿有关，而 $\boldsymbol H_{22}$ 只和路标点有关。

当遍历 $i,j$ 时，以下事实总是成立的：

1. 不管 $i,j$ 怎么变，$\boldsymbol H_{11}$ 都是对角阵，只在 $\boldsymbol H_{i,i}$ 处有非零块。
2. 同理，$\boldsymbol H_{22}$ 也是对角阵，只在 $\boldsymbol H_{j,j}$ 处有非零块。
3. 对于 $\boldsymbol H_{12}$ 和 $\boldsymbol H_{21}$，它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定。

这显示了 $\boldsymbol H$ 的稀疏结构。

举一个实例来直观地说明它的情况。

假设一个场景内有 $2$ 个相机位姿 $(\boldsymbol C_1,\boldsymbol C_2)$ 和 $6$ 个路标点 $(\boldsymbol P_1 ,\boldsymbol P_2, \boldsymbol P_3, \boldsymbol P_4, \boldsymbol P_5,\boldsymbol P_6)$。

这些相机和点云所对应的变量为 $\boldsymbol T_i,i= 1,2$ 及 $\boldsymbol p_j,j= 1,\cdots,6$。

相机 $\boldsymbol C_1$ 观测到路标点 $\boldsymbol P_1 ,\boldsymbol P_2, \boldsymbol P_3, \boldsymbol P_4$，相机 $\boldsymbol C_2$ 观测到路标点 $\boldsymbol P_3, \boldsymbol P_4, \boldsymbol P_5,\boldsymbol P_6$。

把这个过程画成示意图，如下图所示。相机和路标以圆形节点表示。如果 $i$ 相机能够观测到 $j$ 点，就在它们对应的节点连上一条边。

可以推出，场景下的BA目标函数应为：

$$\frac{1}{2}\left(\left\|\boldsymbol e_{11}\right\|^{2}+\left\|\boldsymbol e_{12}\right\|^{2}+\left\|\boldsymbol e_{13}\right\|^{2}+\left\|\boldsymbol e_{14}\right\|^{2}+\left\|\boldsymbol e_{23}\right\|^{2}+\left\|\boldsymbol e_{24}\right\|^{2}+\left\|\boldsymbol e_{25}\right\|^{2}+\left\|\boldsymbol e_{26}\right\|^{2}\right)$$

这里的 $\boldsymbol e_{i,j}$ 使用之前定义过的代价函数，即式 $\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{e}_{i j}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|\boldsymbol{z}_{i j}-h\left(\boldsymbol{T}_{i}, \boldsymbol{p}_{j}\right)\right\|^{2}$

以 $\boldsymbol e_{11}$ 为例，它描述了在 $\boldsymbol C_1$ 看到了 $\boldsymbol P_1$ 这件事，与其他的相机位姿和路标无关。

令 $\boldsymbol J_{11}$ 为 $\boldsymbol e_{11}$ 所对应的雅可比矩阵，不难看出 $\boldsymbol e_{11}$ 对相机变量 $\boldsymbol \xi_2$ 和路标点 $\boldsymbol p_2,\cdots,\boldsymbol p_6$ 的偏导都为 $0$。

把所有变量以 $\boldsymbol x=(\boldsymbol \xi_1,\boldsymbol \xi_2,\boldsymbol p_1,\cdots,\boldsymbol p_6)^T$ 的顺序摆放，则有：

$$\boldsymbol{J}_{11}=\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{x}}=\left(\frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{\xi}_{1}}, \mathbf{0}_{2 \times 6}, \frac{\partial \boldsymbol{e}_{11}}{\partial \boldsymbol{p}_{1}}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}, \mathbf{0}_{2 \times 3}\right)$$

为了方便表示稀疏性，用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为 $0$。那么上面的 $\boldsymbol{J}_{11}$ 则可以表示成下图所示的图案。同理，其他的雅可比矩阵也会有类似的稀疏图案。

$\boldsymbol{J}_{11}$ 矩阵的非零块分布图。上方的标记表示矩阵该列所对应的变量。由于相机参数维数比点云参数维数大，所以 $\boldsymbol C_1$ 对应的矩阵块要比 $\boldsymbol P_1$ 对应的矩阵块宽。

为了得到该目标函数对应的雅可比矩阵，将这些 $\boldsymbol{J}_{ij}$ 按照一定顺序列为向量，那么整体雅可比矩阵及相应的 $\boldsymbol H$ 矩阵的稀疏情况就如下图所示。

现在考虑更一般的情况，假如有 $m$ 个相机位姿，$n$ 个路标点。

由于通常路标的数量远远多于相机，于是有 $n\gg m$。

由上面的推理可知，一般情况下的 $\boldsymbol H$ 矩阵如下图所示。它的左上角块显得非常小，而右下角的对角块占据了大量地方。

除此之外，非对角部分则分布着散乱的观测数据。由于它的形状很像箭头，又称为箭头形（Arrow-like）矩阵。

对于具有这种稀疏结构的 $\boldsymbol H$，线性方程 $\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ 的求解在现实当中存在着若干种利用 $\boldsymbol H$ 的稀疏性加速计算的方法。

本节介绍视觉SLAM里一种最常用的手段：Schur 消元。在SLAM研究中也称为Marginalization（边缘化）。

仔细观察上图，发现这个矩阵可以分成 $4$ 个块，和式 $\boldsymbol{H}=\left[\begin{array}{ll} \boldsymbol{H}_{11} & \boldsymbol{H}_{12} \\ \boldsymbol{H}_{21} & \boldsymbol{H}_{22} \end{array}\right]$ 一致。

左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相同，且是一个对角块矩阵。

右下角也是对角块矩阵，每个对角块的维度是路标的维度。

非对角块的结构与具体观测数据相关。

首先将这个矩阵按照下图所示的方式做区域划分，这4个区域正好对应了公式 $\boldsymbol{H}=\boldsymbol{J}^{\mathrm{T}} \boldsymbol{J}=\left[\begin{array}{ll} \boldsymbol{F}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{F}^{\mathrm{T}} \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} \boldsymbol{F} & \boldsymbol{E}^{\mathrm{T}} \boldsymbol{E} \end{array}\right]$ 中的 $4$ 个矩阵块。

为了后续分析方便，记这 $4$ 个块为 $\boldsymbol B,\boldsymbol E, \boldsymbol E^T ,\boldsymbol C$。

于是，对应的线性方程组也可以由 $\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g}$ 变为如下形式：

$$\left[\begin{array}{ll} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]$$

其中 $\boldsymbol B$ 是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。

由于路标数量会远远大于相机变量个数，所以 $\boldsymbol C$ 往往也远大于 $\boldsymbol B$。

三维空间中每个路标点为三维，于是 $\boldsymbol C$ 矩阵为对角块矩阵，每个块为 $3×3$ 矩阵。

对角块矩阵求逆的难度远小于对一般矩阵的求逆难度，因为只需要对那些对角线矩阵块分别求逆即可。

考虑到这个特性，对线性方程组进行高斯消元，目标是消去右上角的非对角部分 $\boldsymbol E$，得：

$$\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \mathbf 0 & \boldsymbol{I} \end{array}\right]\left[\begin{array}{cc} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol C \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \mathbf{0} & \boldsymbol{I} \end{array}\right]\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]$$

整理得：

$$\left[\begin{array}{cc} \boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{\mathrm{T}} & \mathbf 0 \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol C \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w} \\ \boldsymbol{w} \end{array}\right]$$

消元之后，方程组第一行变成和 $\Delta \boldsymbol{x}_p$ 无关的项。

单独把它拿出来，得到关于位姿部分的增量方程：

$$\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{T}\right] \Delta \boldsymbol{x}_{\mathrm{c}}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}$$

这个线性方程的维度和 $\boldsymbol B$ 矩阵一样。

先求解这个方程，然后把解得的 $\Delta \boldsymbol{x}_c$ 代入原方程，求解 $\Delta \boldsymbol{x}_p$

这个过程称为 Marginalization，或者 Schur 消元（Schur Elimination）

相比于直接解线性方程的做法、它的优势在于：

1. 在消元过程中，由于 $\boldsymbol C$ 为对角块，所以 $\boldsymbol C^{-1}$ 容易解出
2. 求解了 $\Delta \boldsymbol{x}_c$ 之后，路标部分的增量方程由 $\Delta \boldsymbol{x}_{p}=\boldsymbol{C}^{-1}\left(\boldsymbol{w}-\boldsymbol{E}^{\mathrm{T}} \Delta \boldsymbol{x}_{\mathrm{c}}\right)$ 给出。这依然用到了 $\boldsymbol C^{-1}$ 易于求解的特性。

于是，边缘化的主要计算量在于求解式 $\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{T}\right] \Delta \boldsymbol{x}_{\mathrm{c}}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}$

将此方程的系数记为 $\boldsymbol S$，它的稀疏性式不规则的，下图显示了对 $\boldsymbol H$ 矩阵进行 Schur 消元后的一个 $\boldsymbol S$ 实例。

$\boldsymbol H$ 矩阵的非对角块处的非零元素对应着相机和路标的关联。

进行了 Schur 消元后 $\boldsymbol S$ 的稀疏性也具有物理意义：$\boldsymbol S$ 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时称为共视（Co-visibility）。

反之，如果该块为零，则表示这两个相机没有共同观测。

于是，$\boldsymbol S$ 矩阵的稀疏性结构当取决于实际观测的结果，无法提前预知。

在实践中，例如 ORB-SLAM 中的 Local Mapping 环节，在做BA的时候刻意选择那些具有共同观测的帧作为关键帧，在这种情况下，Schur 消元后得到的 $\boldsymbol S$ 就是稠密矩阵。

不过，由于这个模块并不是实时执行，所以这种做法也是可以接受的。

但是有另一些方法，例如 DSO、OKVIS 等，它们采用了滑动窗口（Sliding Window）方法。这类方法对每一帧都要求做一次BA来防止误差的累积，因此它们也必须采用一些技巧来保持 $\boldsymbol S$ 矩阵的稀疏性。

从概率角度来看，称这一步为边缘化，是因为实际上把求 $(\Delta \boldsymbol x_c,\Delta \boldsymbol x_p)$ 的问题，转化成了先固定 $\Delta \boldsymbol x_p$，求出 $\Delta \boldsymbol x_c$，再求 $\Delta \boldsymbol x_p$ 的过程。这一步相当于做了条件概率展开：

$$P\left(\boldsymbol{x}_{\mathrm{c}}, \boldsymbol{x}_{p}\right)=P\left(\boldsymbol{x}_{\mathrm{c}} \mid \boldsymbol{x}_{p}\right) P\left(\boldsymbol{x}_{p}\right),$$

结果是求出了关于 $\boldsymbol x_p$ 的边缘分布，故称边缘化。

在前面介绍的边缘化过程中，实际上把所有的路标点都给边缘化了。

根据实际情况，也能选择一部分进行边缘化。

同时，Schur 消元只是实现边缘化的其中一种方式，同样可以使用 Cholesky 分解进行边缘化。

鲁棒核函数

在前面的BA问题中，将最小化误差项的二范数平方和作为目标函数。

这种做法虽然很直观，但存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误的，会发生什么呢？

我们把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。

在算法看来，这相当于突然观测到了一次很不可能产生的数据。这时，在图优化中会有一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。

所以，算法将试图优先调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。

由于这条边的误差真的很大，往往会抹平其他正确边的影响，使优化算法专注于调整一个错误的值。

这显然不是我们希望看到的。

出现这种问题的原因是，当误差很大时，二范数增长得太快。

于是就有了核函数的存在。核函数保证每条边的误差不会大得没边而掩盖其他的边。

具体的方式是，把原先误差的二范数度量替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然无法求导）。因为它们使得整个优化结果更为稳健，所以又叫它们鲁棒核函数（Robust Kernel）。

鲁棒核函数有许多种，例如最常用的 Huber 核：

$$H(e)=\left\{\begin{array}{ll} \frac{1}{2} e^{2} & \text { 当 }|e| \leqslant \delta, \\ \delta\left(|e|-\frac{1}{2} \delta\right) & \text { 其他 } \end{array}\right.$$

当误差 $e$ 大于某个阈值 $\delta$ 后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。

同时，Huber 核函数又是光滑的，可以很方便地求导。

下图显示了 Huber 核函数与二次函数的对比，可见在误差较大时 Huber 核函数增长明显低于二次函数。

除了 Huber 核，还有 Cauchy 核、Tukey 核，等等，g2o和Ceres都提供了一些核函数。

实践中，多数软件库已经实现了细节操作，而需要做的主要是构造BA问题，设置 Schur 消元，然后调用稠密或者稀疏矩阵求解器对变量进行优化。

小结

本讲比较深入地探讨了状态估计问题与图优化的求解。

在经典模型中SLAM可以看成状态估计问题。

如果假设了马尔可夫性，只考虑当前状态，则得到以EKF为代表的滤波器模型。

如若不然，也可以选择考虑所有的运动和观测，它们构成一个最小二乘问题。在只有观测方程的情况下，这个问题称为BA，并可利用非线性优化方法求解。我们仔细讨论了求解过程中的稀疏性问题，指出了该问题与图优化之间的联系。

第10讲 后端 2

第 $9$ 讲重点介绍了以BA为主的图优化。

BA能精确地优化每个相机位姿与特征点位置。

不过在更大的场景中，大量特征点的存在会严重降低计算效率，导致计算量越来越大，以至于无法实时化。

本讲将介绍一种简化的BA：位姿图。

滑动窗口滤波和优化

实际环境下的BA结构

带有相机位姿和空间点的图优化称为BA，它能够有效地求解大规模的定位与建图问题。

这在 SfM 问题中十分有用，但是在SLAM过程中，往往需要控制BA的规模，保持计算的实时性。

倘若计算能力无限，那不妨每时每刻都计算整个BA——但是那不符合现实需要。

现实条件是，必须限制后端的计算时间，比如BA规模不能超过1万个路标点，迭代不超过20次，用时不超过0.5秒，等等。像 SfM 那样用一周时间重建一个城市地图的算法，在SLAM里不见得有效。

控制计算规模的做法有很多，比如从连续的视频中抽出一部分作为关键帧，仅构造关键帧与路标点之间的BA，于是非关键帧只用于定位，对建图则没有贡献。

即便如此，随着时间的流逝，关键帧数量会越来越多，地图规模也将不断增长。像BA这样的批量优化方法，计算效率会（令人担忧地）不断下降。

为了避免这种情况，需要用一定手段控制后端BA的规模。这些手段可以是理论上的，也可以是工程上的。

例如，最简单的控制BA规模的思路，是仅保留离当前时刻最近的 $N$ 个关键帧，去掉时间上更早的关键帧。

于是，我们的BA将被固定在一个时间窗口内，离开这个窗口的则被丢弃。这种方法称为滑动窗口法。

当然，取这 $N$ 个关键帧的具体方法可以有一些改变，例如，不见得必须取时间上最近的，而可以按照某种原则，取时间上靠近，空间上又可以展开的关键帧，从而保证相机即使在停止不动时，BA的结构也不至于缩成一团（这容易导致一些糟糕的退化情况）。

如果在帧与帧的结构上再考虑得深一些，也可以像 ORB-SLAM2 那样，定义一种称为“共视图”（Covisibility graph）的结构。

所谓共视图，就是指那些与现在的相机存在共同观测的关键帧构成的图。

于是，在BA优化时，按照某些原则在共视图内取一些关键帧和路标进行优化，例如，仅优化与当前帧有20个以上共视路标的关键帧，其余部分固定不变。当共视图关系能够正确构造的时候，基于共视图的优化也会在更长时间内保持最优。

滑动窗口也好，共视图也好，大体而言，都是对实时计算的某种工程上的折中。

不过在理论上，它们也引入了一个新问题：

刚才谈到要“丢弃”滑动窗口之外，或者“固定”共视图之外的变量，这个“丢弃”和“固定”具体怎样操作呢？

“固定”似乎很容易理解，只需将共视图之外的关键帧估计值保持不变即可。

但是“丢弃"，是指完全弃置不用，即窗口外的变量完全不对窗口内的变量产生任何影响，还是说窗口外的数据应该对窗口内的有一些影响，但实际上被我们忽略了？如果有影响，这种影响应该是什么样子？它够不够明显，能不能忽略？

接下来，我们就要谈谈这些问题。它们在理论上应该如何处理、以及在工程上能不能做一些简化手段。

滑动窗口法

考虑一个滑动窗口，假设这个窗口有 $N$ 个关键帧，它们的位姿表达为：

$$\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}$$

假设它们在向量空间，即用李代数表达，显然，我们关心这几个关键帧的位置在哪里，以及它们的不确定度如何，这对应着它们在高斯分布假设下的均值协方差。如果这几个关键帧还对应着一个局部地图，则可以顺带着问整个局部系统的均值和方差应该是多少。

设这个滑动窗口中还有 $M$ 个路标点：$\boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{N}$，它们与 $N$ 个关键帧组成了局部地图。

显然可以用第 $9$ 讲介绍的BA方法处理这个滑动窗口，包括建立图优化模型，构建整体的 Hessian 矩阵，然后边缘化所有路标点来加速求解。在边缘化时，考虑关键帧的位姿，即：

$$\left[\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right]^{\mathrm{T}} \sim N\left(\left[\boldsymbol{\mu}_{1}, \ldots, \boldsymbol{\mu}_{N}\right]^{\mathrm{T}}, \boldsymbol{\Sigma}\right)$$

其中 $\boldsymbol{\mu}_{k}$ 为第 $k$ 个关键帧的位姿均值，$\boldsymbol{\Sigma}$ 为所有关键帧的协方差矩阵，那么显然，均值部分就是指BA迭代之后的结果，而 $\boldsymbol{\Sigma}$ 就是对整个BA的 $\boldsymbol H$ 矩阵进行边缘化之后的结果，即第 $9$ 讲提到的矩阵 $\boldsymbol S$。

在滑动窗口中，当窗口结构发生改变，这些状态变量应该如何变化？

这件事情可以分成两部分讨论：

1. 需要在窗口中新增一个关键帧，以及它观测到的路标点。
2. 需要把窗口中一个旧的关键帧删除，也可能删除它观测到的路标点。

这时，滑动窗口法和传统的BA的区别就显现出来了。显然，如果按照传统的BA来处理，那么这仅仅对应于两个不同结构的BA，在求解上没有任何差别。但在滑动窗口的情况下，就要讨论具体的细节问题了。

新增一个关键帧和路标点

考虑在上个时刻，滑动窗口已经建立了 $N$ 个关键帧，也已知道它们服从某个高斯分布，其均值和方差如前所述。

此时，新来了一个关键帧 $\boldsymbol x_{N+1}$，那么整个问题中的变量变为 $N+1$ 个关键帧和更多路标点的集合。

这实际上仍是平凡的，只需按照正常的BA流程处理即可。对所有点进行边缘化时，即得到这 $N+1$ 个关键帧的高斯分布参数。

删除一个旧的关键帧

当考虑删除旧关键帧时，一个理论问题将显现出来。例如要删除旧关键帧 $\boldsymbol x_1$，但是 $\boldsymbol x_1$ 并不是孤立的，它会和其他帧观测到同样的路标。将 $\boldsymbol x_1$ 边缘化之后将导致整个问题不再稀疏。

滑动窗口删除关键帧将破坏路标部分的对角块结构。

在这个例子中，假设 $\boldsymbol x_{1}$ 看到了路标点 $\boldsymbol y_{1}$ 至 $\boldsymbol y_{4}$，于是，在处理之前，BA问题的Hessian矩阵应该像上图中的左图一样，在 $\boldsymbol x_{1}$ 行的 $\boldsymbol y_{1}$ 到 $\boldsymbol y_{4}$ 列存在着非零块，表示 $\boldsymbol x_{1}$ 看到了它们。

这时考虑边缘化 $\boldsymbol x_{1}$，那么 **Schur** 消元过程相当于通过矩阵行和列操作消去非对角线处几个非零矩阵块，显然这将导致右下角的路标点矩阵块不再是非对角矩阵。

这个过程称为边缘化中的填入（Fill-in）。

回顾第 $9$ 讲中介绍的边缘化，当边缘化路标点时，Fill-in 将出现在左上角的位姿块中。不过，因为BA不要求位姿块为对角块，所以稀疏BA求解仍然可行。

但是，当边缘化关键帧时，将破坏右下角路标点之间的对角块结构，这时BA就无法按照先前的稀疏方式迭代求解。这显然是个十分糟糕的问题。

实际上，在早期的EKF滤波器后端中，人们确实保持着一个稠密的Hessian矩阵，这也使得 EKF 后端没法处理较大规模的滑动窗口。

不过，如果对边缘化的过程进行一些改造，也可以保持滑动窗口BA的稀疏性。

例如，在边缘化某个旧的关键帧时，同时边缘化它观测到的路标点。这样，路标点的信息就会转换成剩下那些关键帧之间的共视信息，从而保持右下角部分的对角块结构。

在某些SLAM框架中，边缘化策略会更复杂。例如在 OKVIS 中，会判断要边缘化的那个关键帧，它看到的路标点是否在最新的关键帧中仍能看到。如果不能，就直接边缘化这个路标点；如果能，就丢弃被边缘化关键帧对这个路标点的观测，从而保持BA的稀疏性。

SWF 中边缘化的直观解释

边缘化在概率上的意义就是指条件概率。

所以，当边缘化某个关键帧，即“保持这个关键帧当前的估计值，求其他状态变量以这个关键帧为条件的条件概率”。

所以，当某个关键帧被边缘化，它观测到的路标点就会产生一个“这些路标应该在哪里”的先验信息，从而影响其余部分的估计值。如果再边缘化这些路标点，那么它们的观测者将得到一个“观测它们的关键帧应该在哪里”的先验信息。

从数学上看，当边缘化某个关键帧，整个窗口中的状态变量的描述方式，将从联合分布变成一个条件概率分布。以上面的例子来看，就是说：

$$p\left(\boldsymbol{x}_{1}, \ldots \boldsymbol{x}_{4}, \boldsymbol{y}_{1}, \ldots \boldsymbol{y}_{6}\right)=p\left(\boldsymbol{x}_{2}, \ldots, \boldsymbol{x}_{4}, \boldsymbol{y}_{1}, \ldots \boldsymbol{y}_{6} \mid \boldsymbol{x}_{1}\right) \underbrace{p\left(\boldsymbol{x}_{1}\right)}_{\text {舍去 }} .$$

然后舍去被边缘化部分的信息。在变量被边缘化之后，在工程中就不应再使用它。所以滑动窗口法比较适合VO系统，而不适合大规模建图的系统。

位姿图

位姿图的意义

根据前面的讨论，特征点在优化问题中占据了绝大部分。

实际上，经过若干次观测之后，收敛的特征点位置变化很小，发散的外点则已被剔除。

对收敛点再进行优化，似乎是有些费力不讨好的。

因此，更倾向于在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束，而不再实际地优化它们的位置估计。

沿着这个思路继续思考，我们会想到：是否能够完全不管路标而只管轨迹呢？完全可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系。

通过这种方式，省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图（Pose Graph）。

在BA中特征点数量远大于位姿节点。一个关键帧往往关联了数百个关键点，而实时BA的最大计算规模，即使利用稀疏性，在当前的主流CPU上一般也就是几万个点左右。这就限制了SLAM应用场景。

所以，当机器人在更大范围的时间和空间中运动时，必须考虑一些解决方式：要么像滑动窗口法那样，丢弃一些历史数据；要么像位姿图的做法那样，舍弃对路标点的优化，只保留Pose之间的边。此外，如果有额外测量Pose的传感器，那么位姿图也是一种常见的融合Pose测量的方法。

位姿图的优化

那么，位姿图优化中的节点表示相机位姿，以 $\boldsymbol T_1,\cdots,\boldsymbol T_p$ 来表达。而边则是两个位姿节点之间相对运动的估计，该估计可以来自于特征点法或直接法，也可以来自 GPS 或 IMU 积分。

无论通过哪种手段，假设估计了 $\boldsymbol T_i$ 和 $\boldsymbol T_j$ 之间的一个运动 $\Delta \boldsymbol T_{ij}$

该运动可以有若干种表达方式，取比较自然的一种：

$$\Delta \boldsymbol{\xi}_{i j}=\boldsymbol{\xi}_{i}^{-1} \circ \boldsymbol{\xi}_{j}=\ln \left(\boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee},$$

或按李群的写法：

$$\boldsymbol T_{ij}= \boldsymbol T_i^{-1} \boldsymbol T_j$$

按照图优化的思路，实际中该等式不会精确地成立，因此设立最小二乘误差，讨论误差关于优化变量的导数。

这里，把上式的 $\boldsymbol T_{ij}$ 移至等式右侧，构建误差 $\boldsymbol e_{ij}$：

$$\boldsymbol{e}_{i j}=\Delta \boldsymbol \xi_{i j} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\right)^{\vee}$$

注意优化变量有两个：$\boldsymbol \xi_{i}$ 和 $\boldsymbol \xi_{j}$，因此我们求 $\boldsymbol{e}_{i j}$ 关于这两个变量的导数。

按照李代数的求导方式，给 $\boldsymbol \xi_{i}$ 和 $\boldsymbol \xi_{j}$ 各一个左扰动：$\boldsymbol\delta \boldsymbol \xi_{i}$ 和 $\boldsymbol\delta \boldsymbol \xi_{j}$

于是误差变为：

$$\hat{\boldsymbol{e}}_{i j}=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \exp \left(\left(-\boldsymbol\delta \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\boldsymbol\delta \boldsymbol{\xi}_{j}^{\wedge}\right) \boldsymbol{T}_{j}\right)^{\vee}$$

该式中，两个扰动项被夹在了中间。

为了利用 BCH 近似，希望把扰动项移至式子左侧或右侧。回忆第4讲习题中的伴随性质，即式 $\operatorname{Ad}(\boldsymbol{T})=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t}^{\wedge} \boldsymbol{R} \\ \mathbf{0} & \boldsymbol{R} \end{array}\right]$，有：

$$\exp \left((\operatorname{Ad}(\boldsymbol{T}) \boldsymbol{\xi})^{\wedge}\right)=\boldsymbol{T} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{T}^{-1}$$

稍加变换，有：

$$\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{T}=\boldsymbol{T} \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}^{-1}\right) \boldsymbol{\xi}\right)^{\wedge}\right)$$

该式表明，通过引入一个伴随项，能够“交换”扰动项左右侧的 $\boldsymbol{T}$。

利用它，可以将扰动挪到最右（当然最左亦可），导出右乘形式的雅可比矩阵（挪到左边时形成左乘）：

$$\begin{aligned} \hat {\boldsymbol {e}}_{i j} &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\boldsymbol \delta \boldsymbol{\xi}_{j}^{\wedge}\right) \boldsymbol{T}_{j}\right)^{\vee} \\ &=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j} \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{j}\right)^{\wedge}\right)\right)^{\vee} \\ & \approx \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_{i}^{-1} \boldsymbol{T}_{j}\left[\boldsymbol{I}-\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{i}\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_{j}\right)^{\wedge}\right]\right)^{\vee} \\ & \approx \boldsymbol{e}_{i j}+\frac{\partial \boldsymbol{e}_{i j}}{\partial \delta \boldsymbol{\delta} \boldsymbol{\xi}_{i}} \boldsymbol{\delta} \boldsymbol{\xi}_{i}+\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{j}} \boldsymbol{\delta} \boldsymbol{\xi}_{j} \end{aligned}$$

因此，按照李代数上的求导法则，求出了误差关于两个位姿的雅可比矩阵。

关于 $\boldsymbol{T_i}$ 的：

$$\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{i}}=-\boldsymbol {\mathcal{J}}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right)$$

以及关于 $\boldsymbol{T_j}$ 的：

$$\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_{j}}=-\boldsymbol {\mathcal{J}}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_{j}^{-1}\right)$$

如果觉得这部分求导理解起来有困难，可以回到第4讲温习李代数部分的内容。

由于 $\mathfrak{s e}(3)$ 上的左右雅可比 $\boldsymbol {\mathcal{J}}_{r}$ 形式过于复杂，通常取它们的近似。如果误差接近零，就可以设它们近似为 $\boldsymbol I$ 或

$$\boldsymbol {\mathcal{J}}_{r}^{-1}\left(\boldsymbol{e}_{i j}\right) \approx \boldsymbol{I}+\frac{1}{2}\left[\begin{array}{cc} \boldsymbol\phi_{e}^{\wedge} & \boldsymbol{\rho}_{e}^{\wedge} \\ \mathbf{0} & \boldsymbol\phi_{\boldsymbol{e}}^{\wedge} \end{array}\right]$$

理论上，即使在优化之后，由于每条边给定的观测数据并不一致，误差也不见得近似于零，所以简单地把这里的 $\boldsymbol {\mathcal{J}}_{r}$ 设置为 $\boldsymbol I$ 会有一定的损失。

了解雅可比求导后，剩下的部分就和普通的图优化一样了。简而言之，所有的位姿顶点和位姿—位姿边构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。

记 $\mathcal{E}$ 为所有边的集合，那么总体目标函数为：

$$\min \frac{1}{2} \sum_{i, j \in \mathcal{E}} \boldsymbol{e}_{i j}^{\mathrm{T}} \boldsymbol{\Sigma}_{i j}^{-1} \boldsymbol{e}_{i j}$$

依然可以用高斯牛顿法、列文伯格—马夸尔特方法等求解此问题，除了用李代数表示优化位姿，别的都是相似的。

根据先前的经验，可以用 Ceres 或 g2o 进行求解。