SLAM入门之后端优化

10 后端

10.1 概述

10.2 BA 与图优化

Bundle Adjustment 光束法平差

• BA是指从视觉重建中提炼出最优的 3D 模型和相机参数（内参数和外参数）。从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程 。
• 早期SLAM 的研究者认为包含大量特征点和相机位姿的 BA 计算量过大，不适合实时计算。直到近十年来，人们逐渐认识到 SLAM 问题中 BA 的稀疏特性，才使它能够在实时的场景中使用

10.2.1 BA问题

• 投影模型
  投影的步骤：
  首先，把世界坐标 $p\text{p}p$ 转换到相机坐标 ${P\text{P}P}^{\prime }$ ，这里将用到相机外参数 $(R,t\text{R,t}R,t)$。 然后，将 ${P\text{P}P}^{\prime }$ 投至归一化平面，得到归一化坐标${P\text{P}P}_c$ 。对归一化坐标去畸变（暂时只考虑径向畸变）得到去畸变后的坐标$u_c^{\prime }，v_c^{\prime }$。 最后，根据内参模型，计算像素坐标 $u_s,v_s$。
  
  之前的观测方程 $z\text{z}z=h(x,y\text{x,y}x,y)$ 在这里参数化了。这里的 $x\text{x}x$ 指代此时相机的位姿，即外参 $R,t\text{R,t}R,t$，它对应的李代数为 $\xi \text{ξ}\xi$。路标 $y\text{y}y$ 即这里的三维点 $p\text{p}p$，而观测数据则是像素坐标 $z\text{z}z\triangleq [u_s,v_s{]}^T$。
• 代价函数
  以最小二乘的角度来考虑，此次观测的误差 $e\text{e}e=z\text{z}z-h(\xi \text{ξ}\xi ,p\text{p}p)$。把其他时刻的观测量也考虑进来，设 $z_{ij}$ 为在位姿 ${\xi \text{ξ}\xi }_i$ 处观察路标 ${p\text{p}p}_j$ 产生的数据，则整体的代价函数为：
  $$\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||{e\text{e}e}_{ij}|{|}^2=\frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||{z\text{z}z}_{ij}-h({\xi \text{ξ}\xi }_i,{p\text{p}p}_j)|{|}^2$$
  对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA

10.2.2 BA求解

易判断观测模型 $h({\xi \text{ξ}\xi }_i,{p\text{p}p}_j)$ 不是线性函数。所以用非线性优化手段来优化它。根据非线性优化的思想，应该从某个的初始值开始，不断地寻找下降方向 $\Delta x\text{x}x$ 来找到目标函数的最优解，即不断地求解增量方程 $H\text{H}H\Delta x\text{x}x=g\text{g}g$ 中的增量 $\Delta x\text{x}x$。

• 在整体 BA 目标函数上，把自变量定义成所有待优化的变量：$x\text{x}x=[{\xi \text{ξ}\xi }_1,...,{\xi \text{ξ}\xi }_m,{p\text{p}p}_1,...,{p\text{p}p}_n{]}^T$
• 增量方程中的 $\Delta x\text{x}x$ 则是对整体自变量的增量。在这个意义下，当我们给自变量一个增量时，目标函数变为：$$\frac{1}{2}||f(x\text{x}x+\Delta x\text{x}x)|{|}^2\approx \frac{1}{2}\sum _{i=1}^m\sum _{j=1}^n||{e\text{e}e}_{ij}+{F\text{F}F}_{ij}\Delta {\xi \text{ξ}\xi }_i+{E\text{E}E}_{ij}\Delta {p\text{p}p}_j|{|}^2$$
  ${F\text{F}F}_{ij}$ 表示整个代价函数在当前状态下对相机姿态的偏导数，而 ${E\text{E}E}_{ij}$ 表示该函数对路标点位置的偏导
• 把相机位姿变量放在一起：${x\text{x}x}_c=[{\xi \text{ξ}\xi }_1,...,{\xi \text{ξ}\xi }_m]\in {\mathbb{R}}^{6m}$
  把空间点的变量放在一起: ${x\text{x}x}_p=[{p\text{p}p}_1,...,{p\text{p}p}_n]\in {\mathbb{R}}^{3n}$
  化简得：
  $$\frac{1}{2}||f(x\text{x}x+\Delta x\text{x}x)|{|}^2=\frac{1}{2}||{e\text{e}e}_{ij}+F\text{F}F\Delta {x\text{x}x}_c+E\text{E}E\Delta {x\text{x}x}_p|{|}^2$$
• 该式从一个由很多个小型二次项之和，变成了一个更整体的样子。这里的雅可比矩阵 $E\text{E}E$ 和 $F\text{F}F$ 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩阵，而里头每个小分块，需要由每个误差项的导数 ${F\text{F}F}_{ij}$ 和 ${E\text{E}E}_{ij}$ “拼凑”起来，由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以分块为：$J\text{J}J=[F\text{F}FE\text{E}E]$
• 无论使用 G-N 还是 L-M 方法，最后都将面对增量线性方程：$H\text{H}H\Delta x\text{x}x=g\text{g}g$
  以 G-N 为例，$H\text{H}H={J\text{J}J}^{T}J\text{J}J=\left[\begin{array}{cc}{F\text{F}F}^{T}F\text{F}F& {F\text{F}F}^{T}E\text{E}E\\ {E\text{E}E}^{T}F\text{F}F& {E\text{E}E}^{T}E\text{E}E\end{array}\right]$
• 因为考虑了所有的优化变量，这个线性方程的维度将非常大，包含了所有的相机位姿和路标点。尤其是在视觉 SLAM 中，一个图像就会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对 H 求逆来计算增量方程非常消耗计算资源。幸运地是，这里的 $H\text{H}H$ 矩阵是有一定的特殊结构的，可以加速求解过程。

10.2.3 H矩阵的稀疏性

• 21 世纪视觉 SLAM 的一个重要进展是认识到了矩阵 $H\text{H}H$ 的稀疏结构，并发现该结构可以自然、显式地用图优化来表示。

• $H\text{H}H$ 矩阵的稀疏性是由雅可比 $J\text{J}J(x\text{x}x)$ 引起的。${e\text{e}e}_{ij}$只涉及到第 $i$ 个相机位姿和第 $j$个路标点，对其余部分的变量的导数都为 0。

  • 所以该误差项对应的雅可比矩阵为${J\text{J}J}_{ij}(x\text{x}x)=({0\text{0}0}_{2\times 6},...{0\text{0}0}_{2\times 6},\frac{\partial {e\text{e}e}_{ij}}{\partial {\xi \text{ξ}\xi }_{ij}},{0\text{0}0}_{2\times 6},...{0\text{0}0}_{2\times 3},...{0\text{0}0}_{2\times 3},\frac{\partial {e\text{e}e}_{ij}}{\partial {p\text{p}p}_j},{0\text{0}0}_{2\times 3},...{0\text{0}0}_{2\times 3})$
    这体现了该误差项与其他路标和轨迹无关的特性
    

• 我们设 ${J\text{J}J}_{ij}$ 只在 $i,j$ 处有非零块，那么它对 $H\text{H}H$ 的贡献为 ${J\text{J}J}_{ij}^T{J\text{J}J}_{ij}$，具有示意图上所画的稀疏形式。这个矩阵也仅有四个非零块，位于$(i,i)，(i,j)，(j,i)，(j,j)$。
  对于整体的 H，$H\text{H}H={\sum }_{i,j}{J\text{J}J}_{ij}^T{J\text{J}J}_{ij}$。$i$ 在所有相机位姿中取值， $j$ 在所有路标点中取值，把$H\text{H}H$进行分块得：$H\text{H}H=\left[\begin{array}{cc}{H\text{H}H}_{11}& {H\text{H}H}_{12}\\ {H\text{H}H}_{21}& {H\text{H}H}_{22}\end{array}\right]$
  这里 ${H\text{H}H}_{11}$ 只和相机位姿有关，而 ${H\text{H}H}_{22}$ 只和路标点有关，这显示了 $H\text{H}H$ 的稀疏结构

  • 不管$i,j$怎么变，${H\text{H}H}_{11}$都是对角阵，只在 ${H\text{H}H}_{i,i}$ 处有非零块
  • 同理，${H\text{H}H}_{22}$也是对角阵，只在 ${H\text{H}H}_{j,j}$ 处有非零块
  • ${H\text{H}H}_{12}，{H\text{H}H}_{21}$ 可能是稀疏的，也可能是稠密的，视具体的观测数据而定

• 实例：

  • 假设一个场景内有 2 个相机位姿 $(C_1,C_2)$ 和 6 个路标 $(P_1, P_2, P_3, P_4, P_5, P_6)。这些相机和点云所对应的变量为 ${\xi \text{ξ}\xi }_i,i=1,2$ 以及 ${p\text{p}p}_j,j=1,...,6$。相机 $C_1$ 观测到路标 $P_1,P_2,P_3,P_4$，相机 $C_2$观测到了路标 $P_3,P_4,P_5,P_6$
    

  • 该场景下的 BA 目标函数：$\frac{1}{2}(||{e\text{e}e}_{11}|{|}^2+||{e\text{e}e}_{12}|{|}^2+||{e\text{e}e}_{13}|{|}^2+||{e\text{e}e}_{14}|{|}^2+||{e\text{e}e}_{23}|{|}^2+||{e\text{e}e}_{24}|{|}^2+||{e\text{e}e}_{25}|{|}^2+||{e\text{e}e}_{26}|{|}^2)$

  • 令${J\text{J}J}_{11}$为${e\text{e}e}_{11}$的雅可比矩阵。把所有变量以$x\text{x}x=({\xi \text{ξ}\xi }_1,{\xi \text{ξ}\xi }_2,{p\text{p}p}_1,...,{p\text{p}p}_2)$的顺序摆放，则有：
    ${J\text{J}J}_{11}=\frac{\partial {e\text{e}e}_{11}}{\partial x\text{x}x}=(\frac{\partial {e\text{e}e}_{11}}{\partial {\xi \text{ξ}\xi }_1},{0\text{0}0}_{2\times 6},\frac{\partial {e\text{e}e}_{11}}{\partial {p\text{p}p}_1},{0\text{0}0}_{2\times 3},{0\text{0}0}_{2\times 3},{0\text{0}0}_{2\times 3},{0\text{0}0}_{2\times 3},{0\text{0}0}_{2\times 3})$

  • 为了方便表示稀疏性，我们用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为 0。那么上面的 ${J\text{J}J}_{11}$ 则可以表示成:
    
    为了得到该目标函数对应的雅可比矩阵，我们可以将这些 Jij 按照一定顺序列为向量，那么整体雅可比矩阵以及相应的 H 矩阵的稀疏情况就是图 10-6 中所展示的那样。
    

  • 对于 H 矩阵当中处于非对角线的矩阵块来说，如果该矩阵块非零，则其位置所对应的变量之间会在图中存在一条边。
    
    H 矩阵当中的非对角部分的非零矩阵块可以理解为它对应的两个变量之间存在联系，或者可以称之为约束。于是，我们发现图优化结构与增量方程的稀疏性存在着明显的联系。

• 假如我们有 m 个相机位姿， n 个路标点。由于通常路标数量远远会比相机多，于是有 n ≫ m。由上面推理可知，实际当中的 H 矩阵会像图 所示的那样。它的左上角块显得非常小，而右下角的对角块占据了大量地方。除此之外，非对角部分则分布着散乱的观测数据。由于它的形状很像箭头，又称为箭头形（Arrow-like）矩阵。
  

10.2.4 边缘化

存在着若干种利用 H 的稀疏性加速计算的方法，而本节介绍视觉 SLAM 里一种最常用的手段： Schur 消元 (Schur trick)。在 SLAM 研究中亦称Marginalization（边缘化）。

• 上图所示矩阵可以分为四块，左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相等，且是一个对角块矩阵。右下角也是对角块矩阵，每个对角块的维度是路标的维度。非对角块的结构与具体观测数据相关。将矩阵按照下图方式分为$B\text{B}B,E\text{E}E,{E\text{E}E}^T,C\text{C}C$
  
• 线性方程组变为：$\left[\begin{array}{cc}B\text{B}B& E\text{E}E\\ {E\text{E}E}^T& C\text{C}C\end{array}\right]\left[\begin{array}{c}\Delta {x\text{x}x}_c\\ \Delta {x\text{x}x}_p\end{array}\right]=\left[\begin{array}{c}v\text{v}v\\ w\text{w}w\end{array}\right]$
  B 是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数。由于路标数量会远远大于相机变量个数，所以 C 往往也远大于 B。三维空间中每个路标点为三维，于是 C 矩阵为对角块矩阵，每个块为 3 × 3 维矩阵。对角块矩阵逆的难度远小于对一般矩阵的求逆难度，因为只需要对那些对角线矩阵块分别求逆即可
• 线性方程组进行高斯消元，得：$\left[\begin{array}{cc}B\text{B}B-E\text{E}E{C\text{C}C}^{-1}{E\text{E}E}^T& 0\text{0}0\\ {E\text{E}E}^T& C\text{C}C\end{array}\right]\left[\begin{array}{c}\Delta {x\text{x}x}_c\\ \Delta {x\text{x}x}_p\end{array}\right]=\left[\begin{array}{c}v\text{v}v-E\text{E}E{C\text{C}C}^{-1}w\text{w}w\\ w\text{w}w\end{array}\right]$
• 第一行方程组变成和 ∆xp 无关的项。单独把它拿出来，得到关于位姿部分的增量方程：
  $[B\text{B}B-E\text{E}E{C\text{C}C}^{-1}{E\text{E}E}^T]\Delta {x\text{x}x}_c=v\text{v}v-E\text{E}E{C\text{C}C}^{-1}w\text{w}w$
  求解这个方程，然后把解得的 $∆{x\text{x}x}_c$ 代入到原方程，然后求解$∆{x\text{x}x}_p$
• 将此方程的系数记为$S\text{S}S$， 进行 Schur 消元。它的稀疏性是不规则的，但具有物理意义：$S\text{S}S$ 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点，有时候称为共视（Co-visibility）。反之，如果该块为零，则表示这两个相机没有共同观测。$S\text{S}S$ 矩阵的稀疏性结构当取决于实际观测的结果，我们无法提前预知。
  

从概率角度来看，我们称这一步为边缘化，是因为我们实际上把求 $(∆{x\text{x}x}_c,∆{x\text{x}x}_p)$ 的问题，转化成先求 $∆{x\text{x}x}_c$，再求 $∆{x\text{x}x}_p$ 的过程。这一步相当于做了条件概率展开：$P({x\text{x}x}_c,{x\text{x}x}_p)=P({x\text{x}x}_c)P({x\text{x}x}_p|{x\text{x}x}_c)$ 结果是求出了关于${x\text{x}x}_c$ 的边缘分布，故称边缘化。

10.2.5 鲁棒核函数

• 如果出于误匹配等原因，某个误差项给的数据是错误的，优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。这时，算法会看到一条误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所以，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。
• 原因是当误差很大时，二范数增长得太快了。核函数保证每条边的误差不会大的没边，掩盖掉其他的边具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质因为它们使得整个优化结果更为鲁棒，所以又叫它们为鲁棒核函数（Robust Kernel）
• Huber 核：$$H(e)=\{\begin{array}{llcc}& \frac{1}{2}{e}^2& & \text{if |e|}\le \delta \\ & \delta (|e|-\frac{1}{2}\delta )& & \text{otherwise}\end{array}$$

10.3 位姿图

• 随着机器人的轨迹变长，地图规模增长，BA的计算效率会下降。经过若干次观测之后，收敛的特征点、空间位置估计就会收敛至一个值保持不动，而发散的外点则通常看不到了。对收敛点再进行优化，似乎是有些费力不讨好的。因此，更倾向于在优化几次之后就把特征点固定住，只把它们看作位姿估计的约束，而不再实际地优化它们的位置估计。

• 沿着这个思路往下走，我们会发现：是否能够完全不管路标，而只管轨迹呢？我们完全可以构建一个只有轨迹的图优化，而位姿节点之间的边，可以由两个关键帧之间通过特征匹配之后得到的运动估计来给定初始值。不同的是，一旦初始估计完成，我们就不再优化那些路标点的位置，而只关心所有的相机位姿之间的联系了。通过这种方式，我们省去了大量的特征点优化的计算，只保留了关键帧的轨迹，从而构建了所谓的位姿图（Pose Graph）
  

• 推导

  • 节点表示相机位姿，用 ${\xi \text{ξ}\xi }_1,\cdots ,{\xi \text{ξ}\xi }_n$ 表示。边表示两个位姿节点之间相对运动的估计（来自于特征点法或直接法），比如说${\xi \text{ξ}\xi }_i$和${\xi \text{ξ}\xi }_j$之间的一个运动$\Delta {\xi \text{ξ}\xi }_{i,j}$
  • 可表达为：$\Delta {\xi \text{ξ}\xi }_{i,j}={\xi \text{ξ}\xi }_i^{-1}\circ {\xi \text{ξ}\xi }_j=\ln (\exp ((-{\xi \text{ξ}\xi }_i{)}^{\wedge })\exp ({\xi \text{ξ}\xi }_j{)}^{\wedge }{)}^{\vee }$ 或按李群的写法: $\Delta {T\text{T}T}_{ij}={T\text{T}T}_i^{-1}{T\text{T}T}_j$
  • 实际当中该等式不会精确地成立，因此我们设立最小二乘误差${e\text{e}e}_{ij}$，讨论误差关于优化变量的导数。 优化变量为 ${\xi \text{ξ}\xi }_i,{\xi \text{ξ}\xi }_j$
    ${e\text{e}e}_{ij}=\ln (\Delta {T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}{T\text{T}T}_j{)}^{\vee }=\ln (\exp ((-{\xi \text{ξ}\xi }_{ij}{)}^{\wedge })\exp ((-{\xi \text{ξ}\xi }_i{)}^{\wedge })\exp ({\xi \text{ξ}\xi }_j{)}^{\wedge }{)}^{\vee }$
  • 李代数求导，给 ${\xi \text{ξ}\xi }_i,{\xi \text{ξ}\xi }_j$ 各一个左扰动：$\delta {\xi \text{ξ}\xi }_i,\delta {\xi \text{ξ}\xi }_j$
    $\widehat{{e\text{e}e}_{ij}}=\ln ({T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}\exp ((-{\delta \xi \text{δξ}\delta \xi }_i{)}^{\wedge })\exp ({\delta \xi \text{δξ}\delta \xi }_j{)}^{\wedge }{T\text{T}T}_j{)}^{\vee }$
    • 运用伴随性质后可利用BCH 近似把扰动项移至式子左侧或右侧：$\exp ({\xi \text{ξ}\xi }^{\wedge })T\text{T}T=T\text{T}T\exp ((Ad({T\text{T}T}^{-1})\xi \text{ξ}\xi {)}^{\wedge })$
  • 导出右乘形式的雅可比矩阵
    $$\begin{array}{rl}\widehat{{e\text{e}e}_{ij}}& =\ln ({T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}\exp ((-{\delta \xi \text{δξ}\delta \xi }_i{)}^{\wedge })\exp ({\delta \xi \text{δξ}\delta \xi }_j{)}^{\wedge }{T\text{T}T}_j{)}^{\vee }\\ & =\ln ({T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}\exp ((-{\delta \xi \text{δξ}\delta \xi }_i{)}^{\wedge }){T\text{T}T}_j\exp ((Ad({T\text{T}T}_j^{-1})\delta \xi \text{δξ}\delta \xi {)}_j^{\wedge }{)}^{\vee }\\ & =\ln ({T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}{T\text{T}T}_j\exp ((-Ad({T\text{T}T}_j^{-1}){\delta \xi \text{δξ}\delta \xi }_i{)}^{\wedge }\exp ((Ad({T\text{T}T}_j^{-1}){\delta \xi \text{δξ}\delta \xi }_j{)}^{\wedge }{)}^{\vee }\\ & \approx \ln ({T\text{T}T}_{ij}^{-1}{T\text{T}T}_i^{-1}{T\text{T}T}_j[I\text{I}I-(Ad({T\text{T}T}_j^{-1}{\delta \xi \text{δξ}\delta \xi }_i{)}^{\wedge }+(Ad({T\text{T}T}_j^{-1}{\delta \xi \text{δξ}\delta \xi }_j{)}^{\wedge }]{)}^{\vee }\\ & \approx {e\text{e}e}_{ij}+\frac{\partial {e\text{e}e}_{ij}}{\partial \delta {\xi \text{ξ}\xi }_i}{\delta \xi \text{δξ}\delta \xi }_i+\frac{\partial {e\text{e}e}_{ij}}{\partial \delta {\xi \text{ξ}\xi }_j}{\delta \xi \text{δξ}\delta \xi }_j\end{array}$$
  • 按照李代数上的求导法则,可求出误差关于两个位姿的雅可比矩阵：$\frac{\partial {e\text{e}e}_{ij}}{\partial \delta {\xi \text{ξ}\xi }_i}=-{J\text{J}J}_r^{-1}({e\text{e}e}_{ij})Ad({T\text{T}T}_j^{-1})$， $\frac{\partial {e\text{e}e}_{ij}}{\partial \delta {\xi \text{ξ}\xi }_j}={J\text{J}J}_r^{-1}({e\text{e}e}_{ij})Ad({T\text{T}T}_j^{-1})$
    ${J\text{J}J}_r^{-1}({e\text{e}e}_{ij})\approx I\text{I}I+\frac{1}{2}\left[\begin{array}{cc}{{\varphi }_e\text{ϕe}{\varphi }_e}^{\wedge }& {{\rho }_e\text{ρe}{\rho }_e}^{\wedge }\\ 0& {{\varphi }_e\text{ϕe}{\varphi }_e}^{\wedge }\end{array}\right]$

• 所有的 位姿顶点 和 位姿—位姿边 构成了一个图优化，本质上是一个最小二乘问题，优化变量为各个顶点的位姿，边来自于位姿观测约束。记 $\xi$ 为所有边的集合，那么总体目标函数为：
  $$\underset{\xi }{\min }\frac{1}{2}\sum _{i,j\in \xi }\left({e\text{e}e}_{ij}^T\sum {}_{ij}^{-1}{e\text{e}e}_{ij}\right)$$
  可以用 Gauss-Newton、 Levenberg-Marquardt 等方法求解此问题