深刻理解图优化与g2o：图优化篇

前言

　　本节咱们将深刻介绍视觉slam中的主流优化方法——图优化（graph-based optimization）。下一节中，介绍一下很是流行的图优化库：g2o。

　　关于g2o，我13年写过一个文档，然而随着本身理解的加深，愈加感受不满意。本着对读者更负责任的精神，本文给你们从新讲一遍图优化和g2o。除了这篇文档，读者还能够找到一篇关于图优化的博客： http://blog.csdn.net/heyijia0327 那篇文章有做者介绍的一个简单案例，而本文则更注重对图优化和g2o的理解与评注。

　　本节主要介绍图优化的数学理论，下节再讲g2o的组成方式及使用方法。

---

预备知识：优化

　　图优化本质上是一个优化问题，因此咱们先来看优化问题是什么。

　　优化问题有三个最重要的因素：目标函数、优化变量、优化约束。一个简单的优化问题能够描述以下：\[ \begin{equation} \min\limits_{x} F(x) \end{equation}\] 其中$x$为优化变量，而$F(x)$为优化函数。此问题称为无约束优化问题，由于咱们没有给出任何约束形式。因为slam中优化问题多为无约束优化，因此咱们着重介绍无约束的形式。

　　当$F(x)$有一些特殊性质时，对应的优化问题也能够用一些特殊的解法。例如，$F(x)$为一个线性函数时，则为线性优化问题（不过线性优化问题一般在有约束情形下讨论）。反之则为非线性优化。对于无约束的非线性优化，若是咱们知道它梯度的解析形式，就能直接求那些梯度为零的点，来解决这个优化：

\[\begin{equation} \frac{{dF}}{{dx}} = 0 \end{equation}\]

　　梯度为零的地方多是函数的极大值、极小值或者鞍点。因为如今$F(x)$的形式不肯定，咱们只好遍历全部的极值点，找到最小的做为最优解。

　　可是咱们为何不这样用呢？由于不少时候$F(x)$的形式太复杂，致使咱们无法写出导数的解析形式，或者难以求解导数为零的方程。所以，多数时候咱们使用迭代方式求解。从一个初值$x_0$出发，不断地致使当前值附近的，能使目标函数降低的方式（反向梯度），而后沿着梯度方向走出一步，从而使得函数值降低一点。这样反复迭代，理论上对于任何函数，都能找到一个极小值点。

　　迭代的策略主要体如今如何选择降低方向，以及如何选择步长两个方面。主要有 Gauss-Newton （GN）法和 Levenberg-Marquardt （LM）法两种，它们的细节能够在维基上找到，咱们不细说。请理解它们主要在迭代策略上有所不一样，可是寻找梯度并迭代则是同样的。

---

图优化

　　所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。

　　图是什么呢？

　　图是由顶点（Vertex）和边（Edge）组成的结构，而图论则是研究图的理论。咱们记一个图为$G=\{ V, E \}$，其中$V$为顶点集，$E$为边集。

　　顶点没什么可说的，想象成普通的点便可。

　　边是什么呢？一条边链接着若干个顶点，表示顶点之间的一种关系。边能够是有向的或是无向的，对应的图称为有向图或无向图。边也能够链接一个顶点（Unary Edge，一元边）、两个顶点（Binary Edge，二元边）或多个顶点（Hyper Edge，多元边）。最多见的边链接两个顶点。当一个图中存在链接两个以上顶点的边时，称这个图为超图（Hyper Graph）。而SLAM问题就能够表示成一个超图（在不引发歧义的状况下，后文直接以图指代超图）。

　　怎么把SLAM问题表示成图呢？

　　SLAM的核心是根据已有的观测数据，计算机器人的运动轨迹和地图。假设在时刻$k$，机器人在位置$x_k$处，用传感器进行了一次观测，获得了数据$z_k$。传感器的观测方程为：

\[ \begin{equation}{z_k} = h\left( {{x_k}} \right) \end{equation} \]

　　因为偏差的存在，$z_k$不可能精确地等于$h(x_k)$，因而就有了偏差：

　　\[ \begin{equation} {e_k} = {z_k} - h\left( {{x_k}} \right) \end{equation} \]

　　那么，若是咱们以$x_k$为优化变量，以$ \min\limits_x F_k (x_k) = \| e_k \| $为目标函数，就能够求得$x_k$的估计值，进而获得咱们想要的东西了。这实际上就是用优化来求解SLAM的思路。

　　你说的优化变量$x_k$，观测方程$z_k = h (x_k)$等等，它们具体是什么东西呢？

　　这个取决于咱们的参数化（parameterazation）。$x$能够是一个机器人的Pose（6自由度下为 $4\times 4$的变换矩阵$\mathbf{T}$ 或者 3自由度下的位置与转角$[x,y,\theta]$，也能够是一个空间点（三维空间的$[x,y,z]$或二维空间的$[x,y]$）。相应的，观测方程也有不少形式，如：

• 机器人两个Pose之间的变换；
• 机器人在某个Pose处用激光测量到了某个空间点，获得了它离本身的距离与角度；
• 机器人在某个Pose处用相机观测到了某个空间点，获得了它的像素坐标；

　　一样，它们的具体形式不少样化，这容许咱们在讨论slam问题时，不局限于某种特定的传感器或姿态表达方式。

　　我明白优化是什么意思了，可是它们怎么表达成图呢？

　　在图中，以顶点表示优化变量，以边表示观测方程。因为边能够链接一个或多个顶点，因此咱们把它的形式写成更广义的 $z_k = h(x_{k1}, x_{k2}, \ldots )$，以表示不限制顶点数量的意思。对于刚才提到的三种观测方程，顶点和边是什么形式呢？

• 机器人两个Pose之间的变换；——一条Binary Edge（二元边），顶点为两个pose，边的方程为${T_1} = \Delta T \cdot {T_2}$。
• 机器人在某个Pose处用激光测量到了某个空间点，获得了它离本身的距离与角度；——Binary Edge，顶点为一个2D Pose：$[x,y,\theta]^T$和一个Point：$[\lambda_x, \lambda_y]^T$，观测数据是距离$r$和角度$b$，那么观测方程为：
  \[ \begin{equation}
      \left[
      {\begin{array}{*{20}{c}}
        {r}\\
        {b}
      \end{array}}
  \right] = \left[ {
      \begin{array}{*{20}{c}}
        {\sqrt {{{({\lambda _x} - x)}^2} + {{({\lambda _y} - y)}^2}}}\\
        {{{\tan }^{ - 1}}\left( {\frac{{{\lambda _y} - y}}{{{\lambda _x} - x}}} \right) - \theta  }
      \end{array}}
  \right]
  \end{equation}\]
• 机器人在某个Pose处用相机观测到了某个空间点，获得了它的像素坐标；——Binary Edge，顶点为一个3D Pose：$T$和一个空间点$\mathbf{x} = [x,y,z]^T$，观测数据为像素坐标$z=[u,v]^T$。那么观测方程为：\[ \begin{equation} z = C  ( R \mathbf{x} + t ) \end{equation} \]

 　　$C$为相机内参，$R,t$为旋转和平移。

　　举这些例子，是为了让读者更好地理解顶点和边是什么东西。因为机器人可能使用各类传感器，故咱们不限制顶点和边的参数化以后的样子。好比我（丧心病狂地在小萝卜身上）既加了激光，也用相机，还用了IMU，轮式编码器，超声波等各类传感器来作slam。为了求解整个问题，个人图中就会有各类各样的顶点和边。可是无论如何，都是能够用图来优化的。

（暗黑小萝卜 小眼神degined by Orchid Zhang）_{之后找不到工做我就去当插画算了……}

---

图优化怎么作

　　如今让咱们来仔细看一看图优化是怎么作的。假设一个带有$n$条边的图，其目标函数能够写成：

\[ \begin{equation} \min\limits_{x} \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k},{z_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k},{z_k}} \right)} \end{equation}\]

 　　关于这个目标函数，咱们有几句话要讲。这些话都是很重要的，请读者仔细去理解。

1. $e$ 函数在原理上表示一个偏差，是一个矢量，做为优化变量$x_k$和$z_k$符合程度的一个度量。它越大表示$x_k$越不符合$z_k$。可是，因为目标函数必须是标量，因此必须用它的平方形式来表达目标函数。最简单的形式是直接作成平方：$e(x,z)^T e(x,z)$。进一步，为了表示咱们对偏差各份量重视程度的不同，还使用一个信息矩阵 $\Omega$ 来表示各份量的不一致性。
2. 信息矩阵 $\Omega$ 是协方差矩阵的逆，是一个对称矩阵。它的每一个元素$ \Omega_{i,j}$做为$e_i e_j$的系数，能够当作咱们对$e_i, e_j$这个偏差项相关性的一个预计。最简单的是把$\Omega$设成对角矩阵，对角阵元素的大小代表咱们对此项偏差的重视程度。
3. 这里的$x_k$能够指一个顶点、两个顶点或多个顶点，取决于边的实际类型。因此，更严谨的方式是把它写成$e_k ( z_k, x_{k1}, x_{k2}, \ldots )$，可是那样写法实在是太繁琐，咱们就简单地写成如今的样子。因为$z_k$是已知的，为了数学上的简洁，咱们再把它写成$e_k(x_k)$的形式。

　　因而整体优化问题变为$n$条边加和的形式：

\[ \begin{equation} \min F(x) = \sum\limits_{k = 1}^n {{e_k}{{\left( {{x_k}} \right)}^T}{\Omega _k}{e_k}\left( {{x_k}} \right)} \end{equation}\]

　　重复一遍，边的具体形式有不少种，能够是一元边、二元边或多元边，它们的数学表达形式取决于传感器或你想要描述的东西。例如视觉SLAM中，在一个相机Pose $T_k$ 处对空间点$\mathbf{x}_k$进行了一次观测，获得$z_k$，那么这条二元边的数学形式即为$${e_k}\left( {{x_k},{T_k},{z_k}} \right) = {\left( {{z_k} - C\left( {R{x_k} + t} \right)} \right)^T}{\Omega _k}\left( {{z_k} - C\left( {R{x_k} - t} \right)} \right)$$ 单个边其实并不复杂。

　　如今，咱们有了一个不少个节点和边的图，构成了一个庞大的优化问题。咱们并不想展开它的数学形式，只关心它的优化解。那么，为了求解优化，须要知道两样东西：一个初始点和一个迭代方向。为了数学上的方便，先考虑第$k$条边$e_k(x_k)$吧。

　　咱们假设它的初始点为${{\widetilde x}_k}$，而且给它一个$\Delta x$的增量，那么边的估计值就变为$F_k ( {\widetilde x}_k + \Delta x )$，而偏差值则从 $e_k(\widetilde x)$ 变为 $e_k({\widetilde x}_k + \Delta x )$。首先对偏差项进行一阶展开：

\[ \begin{equation} {e_k}\left( {{{\widetilde x}_k} + \Delta x} \right) \approx {e_k}\left( {{{\widetilde x}_k}} \right) + \frac{{d{e_k}}}{{d{x_k}}}\Delta x = {e_k} + {J_k}\Delta x\end{equation} \]

　　这是的$J_k$是$e_k$关于$x_k$的导数，矩阵形式下为雅可比阵。咱们在估计点附近做了一次线性假设，认为函数值是可以用一阶导数来逼近的，固然这在$\Delta x$很大时候就不成立了。

　　因而，对于第$k$条边的目标函数项，有：

　　进一步展开：

$$\begin{array}{lll}{F_k}\left( {{{\widetilde x}_k} + \Delta x} \right) &=& {e_k}{\left( {{{\widetilde x}_k} + \Delta x} \right)^T}{\Omega _k}{e_k}\left( {{{\widetilde x}_k} + \Delta x} \right)\\
& \approx & {\left( {{e_k} + {J_k}\Delta x} \right)^T}{\Omega _k}\left( {{e_k} + J\Delta x} \right)\\
&=& e_k^T{\Omega _k}{e_k} + 2e_k^T{\Omega _k}{J_k}\Delta x + \Delta {x^T}J_k^T{\Omega _k}{J_k}\Delta x\\ &=& {C_k} + 2{b_k}\Delta x + \Delta {x^T}{H_k}\Delta x
\end{array}$$

 　　在熟练的同窗看来，这个推导就像$(a+b)^2=a^2+2ab+b^2$同样简单（事实上就是好吧）。最后一个式子是个定义整理式，咱们把和$\Delta x$无关的整理成常数项 $C_k$ ，把一次项系数写成 $2b_k$ ，二次项则为 $H_k$（注意到二次项系数实际上是Hessian矩阵）。

　　请注意 $C_k$ 实际就是该边变化前的取值。因此在$x_k$发生增量后，目标函数$F_k$项改变的值即为$$\Delta F_k = 2b_k \Delta x + \Delta x^T H_k \Delta x. $$

　　咱们的目标是找到$\Delta x$，使得这个增量变为极小值。因此直接令它对于$\Delta x$的导数为零，有：

\[ \begin{equation} \frac{{d{F_k}}}{{d\Delta x}} = 2b + 2{H_k}\Delta x = 0 \Rightarrow {H_k}\Delta x =  - b_k \end{equation} \]

　　因此归根结底，咱们求解一个线性方程组：\[ \begin{equation} H_k \Delta x = -b_k \end{equation} \] 

　　若是把全部边放到一块儿考虑进去，那就能够去掉下标，直接说咱们要求解$$ H \Delta x = -b. $$

　　原来算了半天它只是个线性的！线性的谁不会解啊！

　　读者固然会有这种感受，由于线性规划是规划中最为简单的，连小学生都会解这么简单的问题，为什么21世纪前SLAM不这样作呢？——这是由于在每一步迭代中，咱们都要求解一个雅可比和一个海塞。而一个图中常常有成千上万条边，几十万个待估计参数，这在之前被认为是没法实时求解的。

　　那为什么后来又能够实时求解了呢？

　　SLAM研究者逐渐认识到，SLAM构建的图，并不是是全连通图，它每每是很稀疏的。例如一个地图里大部分路标点，只会在不多的时刻被机器人看见，从而创建起一些边。大多数时候它们是看不见的（就像后宫怨女同样）。体如今数学公式中，虽然整体目标函数$F(x)$有不少项，但某个顶点$x_k$就只会出如今和它有关的边里面！

　　这会致使什么？这致使许多和$x_k$无关的边，好比说$e_j$，对应的雅可比$J_j$就直接是个零矩阵！而整体的雅可比$J$中，和$x_k$有关的那一列大部分为零，只有少数的地方，也就是和$x_k$顶点相连的边，出现了非零值。

 

　　相应的二阶导矩阵$H$中，大部分也是零元素。这种稀疏性能很好地帮助咱们快速求解上面的线性方程。出于篇幅咱们先不细说这是如何作到的了。稀疏代数库包括SBA、PCG、CSparse、Cholmod等等。g2o正是使用它们来求解图优化问题的。

　　要补充一点的是，在数值计算中，咱们能够给出雅可比和海塞的解析形式进行计算，也可让计算机去数值计算这两个阵，而咱们只须要给出偏差的定义方式便可。

---

 流形

　　等一下老师！上面推导还有一个问题！

　　很好，小萝卜同窗，请说说是什么问题。

　　咱们在讨论给目标函数$F(x)$一个增量$\Delta x$时，直接就写成了$F(x+\Delta x)$。可是老师，这个加法可能没有定义！

　　小萝卜同窗看到了一个严重的问题，这确实是在先前的讨论中忽略掉了。因为咱们不限制顶点的类型，$x$在参数化以后，极可能是没有加法定义的。

　　最简单的就是常见的四维变换矩阵$T$或者三维旋转矩阵$R$。它们对加法并不封闭，由于两个变换阵之和并非变换阵，两个正交阵之和也不是正交阵。它们乘法的性质很是好，可是确实没有加法，因此也不能像上面讨论的那样去求导。

　　可是，若是图优化不能处理$SE(3)$或$SO(3)$中的元素，那将是十分使人沮丧的，由于SLAM要估计的机器人轨迹必须用它们来描述啊。

　　回想咱们先前讲过的李代数知识。虽然李群 $SE(3)$ 和 $SO(3)$ 是没有加法的，可是它们对应的李代数 $\mathfrak{se}(3), \mathfrak{so}(3)$ 有啊！ 数学一点地说，咱们能够求它们在正切空间里的流形上的梯度！若是读者以为理解困难，咱们就说，经过指数变换和对数变换，先把变换矩阵和旋转矩阵转换成李代数，在李代数上进行加法，而后再转换到本来的李群中。这样咱们就完成了求导。

　　这样的好处是咱们彻底不用从新推导公式。这件事比咱们想的更加简单。在程序里，咱们只需从新定义一个优化变量$x$的增量加法便可。若是$x$是一个$SE(3)$里的变换矩阵，咱们就遵照刚才讲的李代数转换方式。固然，若是$x$是其余什么奇怪的东东，只要定义了它的加法，程序就会自动去计算如何求它的雅可比。

---

 核函数

　　又是核函数！——学过机器学习课程的同窗确定要这样讲。

　　可是很遗憾，图优化中也有一种核函数。 引入核函数的缘由，是由于SLAM中可能给出错误的边。SLAM中的数据关联让科学家头疼了很长时间。出于变化、噪声等缘由，机器人并不能肯定它看到的某个路标，就必定是数据库中的某个路标。万一认错了呢？我把一条本来不该该加到图中的边给加进去了，会怎么样？

　　嗯，那优化算法可就慒逼了……它会看到一条偏差很大的边，而后试图调整这条边所链接的节点的估计值，使它们顺应这条边的无理要求。因为这个边的偏差真的很大，每每会抹平了其余正确边的影响，使优化算法专一于调整一个错误的值。

　　因而就有了核函数的存在。核函数保证每条边的偏差不会大的没边，掩盖掉其余的边。具体的方式是，把原先偏差的二范数度量，替换成一个增加没有那么快的函数，同时保证本身的光滑性质（否则无法求导啊！）。由于它们使得整个优化结果更为鲁棒，因此又叫它们为robust kernel（鲁棒核函数）。

　　不少鲁棒核函数都是分段函数，在输入较大时给出线性的增加速率，例如cauchy核，huber核等等。固然具体的咱们也不展开细说了。

　　核函数在许多优化环境中都有应用，博主我的印象较深的时当年有一大堆人在机器学习算法里加各类各样的核，咱们如今用的svm也会带个核函数。

---

 小结

　　最后总结一下作图优化的流程。

1. 选择你想要的图里的节点与边的类型，肯定它们的参数化形式；
2. 往图里加入实际的节点和边；
3. 选择初值，开始迭代；
4. 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
5. 求解稀疏线性方程$H_k \Delta x = -b_k$，获得梯度方向；
6. 继续用GN或LM进行迭代。若是迭代结束，返回优化值。

　　实际上，g2o能帮你作好第3-6步，你要作的只是前两步而已。下节咱们就来尝试这件事。