视觉SLAM十四讲笔记-6-1

视觉SLAM十四讲笔记-6-1

非线性优化

主要目标：
１. 理解最小二乘法的含义和处理方式；
２. 理解高斯牛顿法(Gauss-Newton’s method)，列文伯格-马夸尔特方法(Levenburg-Marquadt’s method)等下降策略；
3. 学习Ceres库和g2o库的基本使用用法；
在前几讲中，介绍了经典SLAM模型的运动方程和观测方程．现在已经知道，运动方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化．观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿．这即为经典SLAM在视觉情况下的具体表达．
然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的．尽管相机可以非常好地符合针孔模型，但遗憾的是，得到的数据是受各种未知噪声影响的．即使有高精度的相机，运动方程和观测方程也只能近似成立．所以，与其假设数据必须符合方程，不如讨论如何在有噪声的情况中进行精确的状态估计．
解决状态估计问题需要一定程度的最优化背景知识．本节将介绍基本的无约束非线性优化方法，同时介绍优化库g2o和Ceres的使用方式．

6.1 状态估计问题

6.1.1 批量状态估计与最大后验估计

回顾第二章的经典SLAM模型．它由一个运动方程和一个观测方程构成．
$$\{\begin{array}{l}{\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)+{\mathbf{w}}_k\\ {\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}\end{array}$$
上述两个方程：
运动方程：根据 $u_k$ 的输入，在噪声 $w$ 的影响下，位置 $x$ 的变化；观测方程:在 $x_k$ 处对路标 $y_j$ 进行观测，在噪声 $v$ 的影响下产生一个观测数据 $z_{kj}$．
通过第四章的学习可知，这里的 $x_k$ 是相机的位姿，可以用 $SE(3)$ 来描述．至于观测方程，从第五章可知，可以用针孔相机模型来表示．
下面讨论具体参数化形式．
首先，位姿变量 $x_k$ 可以由 $T_k\in SE(3)$ 表达．
然后，观测方程由针孔模型给出，假设在 $x_k$ 处对路标 $y_j$ 进行了一次观测，对应到图像上的像素位置为 $z_{k,j}$，那么观测方程可以表示成
$$s{z}_{k,j}=K(R_k{y}_j+t_k)$$
其中 $K$ 为相机内参，$s$ 为像素点的距离，也就是 $(R_k{y}_j+t_k)$ 的第三个分量．如果用变换矩阵 $T_k$ 描述位姿，那么路标点 $y_j$ 必须以齐次坐标来描述．计算完成后要转换为非齐次坐标．
接下来，考虑数受到噪声影响后会发生什么改变，在运动和观测方程中，通常假设两个噪声$w_k$ 和 $v_{k,j}$ 满足零均值的高斯分布，像这样：
$${\mathbf{w}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right),{\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$$
其中 $\mathcal{N}$ 代表高斯分布，$0$ 代表零均值，${\mathbf{R}}_k,{\mathbf{Q}}_{k,j}$ 为协方差矩阵．
在这些噪声的影响下，希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$ 以及它们的概率分布，这构成了一个状态估计问题．
处理这个状态估计问题的方法大致分为两种．
1.由于在SLAM过程中，这些数据是随时间逐渐到来的，所以，应该持有一个当前时刻的估计状态，然后用新的数据来更新它．这种方式称为增量/渐进(incremental)的方法，或者叫滤波器．
2.另一种方式，是把数据＂攒＂起来一并处理，这种方式称为批量(batch)的方法．例如，可以把0到k时刻所有的输入和观测数据都放在一起，问在这样的输入和观测下，如何估计整个0到k时刻的轨迹与地图尼?
这两种不同的处理方式印出来很多不同的估计手段．大体来说，增量方法仅关心当前时刻的状态估计 $x_k$, 而对之前的状态则不多考虑；相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器,而成为视觉SLAM的主流方法．
极端情况下，可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这正是SfM(Structure from Motion)的主流做法．但是，这种极端情况不是实时的，不符合SLAM的运用场景．所以在SLAM中，使用的方法通常是一些折中的手段．例如，可以固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化，这是后面要讲到的滑动窗口估计法．
本节先重点介绍以非线性优化为主的批量优化方法，将卡尔曼滤波器及更深入的知识留到介绍后端的章节中再进行介绍．
接下来讨论一下批量优化方法：
考虑从1到N的所有时刻，并假设有M个路标点．定义所有时刻的机器人位姿和路标点坐标为：
x = { x 1 , . . . , x N } , y = { y 1 , . . . , y m } x =\left \{ x_1,...,x_N \right \} ,y = \left \{ y_1, ..., y_m \right \} x={x1​,...,xN​},y={y1​,...,ym​}
用不带下标的 $u$ 表示所有时刻的输入， $z$ 表示所有时刻的观测数据．那么，对于机器人的状态估计来说，从概率学的观点来说，就是已知输入数据 $u$ 和观测数据 $z$ 的条件下，求状态 $x$ 和 $y$ 的条件概率分布：
$$P(x,y|z,u)$$
特别地，当不知道控制输入，只有一张张图像时，即只考虑观测方程带来的数据时，相当于估计 $P(x,y|z)$的条件概率分布，此问题被称为 $SfM$.
为了估计状态变量的条件分布，利用贝叶斯法则，有：
概率论中有如下计算公式：
$$P(A|B)=\frac{P(AB)}{P(B)}=\frac{P(B|A)P(A)}{P(B)}$$
因此：
$$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\frac{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})}{P(\mathbf{z},\mathbf{u})}\propto \underset{\text{似 然 }}{\underbrace{P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})}}\underset{\text{先 验 }}{\underbrace{P(\mathbf{x},\mathbf{y})}}$$
上式称为贝叶斯法则，$P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})$称为后验概率，右侧的 $P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$称为似然，另一部分 $P(\mathbf{x},\mathbf{y})$称为先验．
直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下后验概率最大化则是可行的．(使得在该状态下后验概率的可能性更大，即转化为一个状态最优估计问题)
$$(\mathbf{x},\mathbf{y}{)}^{\ast }{}_{\mathrm{M}\mathrm{A}\mathrm{P}}=\arg \max P(\mathbf{x},\mathbf{y}\mid \mathbf{z},\mathbf{u})=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})P(\mathbf{x},\mathbf{y})$$
贝叶斯法则中分母与 $x,y$ 无关，因此可以忽略．贝叶斯法则告诉我们求解最大后验概率等价于最大似然和先验的乘积．
进一步，当不知道机器人位姿或者路标大概在什么地方时，此时就没有了先验．那么，可以求解最大似然估计（MaximizeLikelihood Estimation，MLE）：
$$(\mathbf{x},\mathbf{y}{)}^{\ast }{}_{\mathrm{M}\mathrm{L}\mathrm{E}}=\arg \max P(\mathbf{z},\mathbf{u}\mid \mathbf{x},\mathbf{y})$$
直观地讲，似然是指"在现在的位姿下，可能产生怎样的观测数据"．由于观测数据是已知的，因此最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测到的数据”．这就是最大似然估计的直观意义．

6.1.2 最小二乘的引出

这里根据观测方程来推导观测模型的最大似然估计
如何求解最大似然估计尼?在高斯分布的假设下，最大似然能够有较为简单的形式．回顾观测模型，对于每一次观测：
$${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right)+{\mathbf{v}}_{k,j}$$
噪声项${\mathbf{v}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_{k,j}\right)$，所以观测数据的条件概率为：
$$P(z_{j,k}|x_k,y_j)=N(h(y_j,x_k),Q_{k,j})$$
它依然是一个高斯分布．考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然．

高斯分布在负对数下具有较好的数学形式．考虑任意高维高斯分布 $x\sim \mathcal{N}(\mu ,\Sigma )$，它的概率密度函数展开形式为：
$$P(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N\det (\mathbf{\Sigma })}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)$$
对其取负对数，变为：
$$-\ln (P(\mathbf{x}))=\frac{1}{2}\ln \left((2\pi {)}^N\det (\mathbf{\Sigma })\right)+\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^{\mathrm{T}}{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })$$
因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化．在最小化上式的 $x$ 时，第一项与 $x$ 无关，可以略去．于是，只要最小化右侧的二次型项，就能得到对状态的最大似然估计．带入SLAM的观测模型，相当于在求：
$$\begin{array}{rl}{\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)}^{\ast }& =\arg \max \mathcal{N}\left(h\left({\mathbf{y}}_j,{\mathbf{x}}_k\right),{\mathbf{Q}}_{k,j}\right)\\ & =\arg \min \left({\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)}^{\mathrm{T}}{\mathbf{Q}}_{k,j}^{-1}\left({\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\right)\right)\end{array}$$
该式等价于最小化噪声项(即误差)的一个二次型．这个二次型称为马哈拉诺斯距离(Mahalanobis distance)，又叫马氏距离．
这也可以看成由 $Q_{k,j}^{-1}$ 加权之后的欧氏距离(二范数)．$Q_{k,j}^{-1}$也叫作信息矩阵，即高斯分布协方差矩阵的逆．

上式相当于观测方程独立的最大似然估计
现在考虑批量时刻的数据．通常假设各个时刻的输入和观测是相互独立的，这意味着各个时刻之间是相互独立的，各个观测之间是独立的，并且输入和观测是独立的．于是可以对联合分布做因式分解：
$$P(z,u|x,y)=\prod _{k}P(u_k|x_{k-1,x_k})\prod _{k,j}P(z_{k,j}|x_k,y_j)$$
这说明可以独立地处理各个时刻的运动和观测．定义各次输入和观测数据与模型之间的误差：
$$\begin{array}{rl}& {\mathbf{e}}_{u,k}={\mathbf{x}}_k-f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k\right)\\ & {\mathbf{e}}_{z,j,k}={\mathbf{z}}_{k,j}-h\left({\mathbf{x}}_k,{\mathbf{y}}_j\right)\end{array}$$
那么，最小化所有时刻估计值与真实读数之间的马氏距离．等价于求最大似然估计．负对数允许把乘积变成求和：
$$minJ(\mathbf{x},\mathbf{y})=\sum _k{e}_{u,k}^T{\mathbf{R}}_k^{-1}{e}_{u,k}+\sum _k\sum _j{e}_{z,k,j}^T{Q}_{k,j}^{-1}{e}_{z,k,j}$$
这样就得到了一个最小二乘问题．它的解等价于状态的最大似然估计．

上式相当于运动方程和观测方程独立的最大似然估计
直观上看，由于噪声的存在，当把估计的轨迹与地图带入SLAM的运动，观测方程中时，并不会完美地成立．这时怎么办?通过对状态的估计值进行微调，使得整体的误差下降一些．当然，这个下降也有限度，它一般会达到一个极小值．这就是一个典型的非线性优化的过程．
通过观察上式，发现SLAM中的最小二乘问题具有一定的特定结构：
1.首先，整个问题的目标函数有许多个误差(加权的)二次型组成．虽然总体的状态量维数很高，但每个误差项都是简单的，仅与一两个状态变量相关．例如，运动方程只与 $x_k,x_{k-1}$ 有关，观测误差只与 $x_k,y_j$ 有关．这种关系会让整个问题有一种稀疏的形式．
2.其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题．但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束，即需在问题中加入 $R^{T}R=I$ 且 $det(R)=1$约束．额外的约束会使得优化变得困难．
3.使用二次型度量误差．误差的分布将影响此项在整个问题中的权重．之后会讨论这个问题．
这节介绍了最小二乘问题的引出，如何求解这个最小二乘问题，需要一些非线性优化的基础知识．

6.1.3 例子：批量状态估计

现在通过一个例子来说明如何求解最小二乘问题．
考虑一个非常简单的离散时间系统：
$$\begin{gathered} x_k=x_{k-1}+u_k+w_k,{\mathbf{w}}_k\sim \mathcal{N}\left(0,{\mathbf{Q}}_k\right) \\ {z}_k=x_k+n_k,{\mathbf{n}}_k\sim \mathcal{N}\left(0,{\mathbf{R}}_k\right) \end{gathered}$$
这可以表达一辆沿 $x$ 轴前进或者后退的汽车．第一个公式为运动方程， $u_k$ 为输入， $w_k$ 为噪声；第二个公式为观测方程，$z_k$ 为汽车位置的测量，取时间 $k=1,2,3$．现在希望根据已有的 $u,z$ 进行状态估计．
设初始状态 $x_0$ 已知，下面推导批量状态的最大似然估计．
首先，令批量状态变量为 $x=[x_0,x_1,x_2,x_3{]}^T$，令批量观测为 $z=[z_1,z_2,z_3{]}^T$，按照同样方法定义 $u=[u_1,u_2,u_3{]}^T$．按照先前的推导，知道最大似然估计为：
$$x_{map}^{\ast }=argmaxP(x|u,z)=argmaxP(u,z|x)=\prod _{k=1}^{3}P(u_k|x_{k-1},x_k)\prod _{k=1}^{3}P(z_k|x_k)$$
对于具体的每一项，比如运动方程：
$$P(u_k|x_{k-1},x_k)=\mathcal{N}(x_k-x_{k-1},{\mathbf{Q}}_k)$$
观测方程也是类似的：
$$P(z_k|x_k)=\mathcal{N}(x_k,{\mathbf{R}}_k)$$
根据这些方法，就能够实际地解决上面的批量状态估计问题．根据之前的叙述，可以构建误差变量：
$$e_{u,k}=x_k-x_{k-1}-u_k,e_{z,k}=z_k-x_k$$
于是最小二乘的目标函数是：
$$min\sum _{k=1}^3{e}_{u,k}^T{\mathbf{Q}}_k^{-1}{e}_{u,k}+\sum _{k=1}^3{e}_{z,k}^T{\mathbf{R}}_k^{-1}{e}_{z,k}$$
此外，这个系统是线性系统，可以很容易地将它写成向量的形式，定义向量 $y=[-u,z{]}^T$，那么可以写成矩阵 $H$，使得：
$$y-Hx=e\sim \mathcal{N}(0,\Sigma )$$

$$\left[\begin{array}{c}-u_1\\ -u_2\\ -u_3\\ -u_4\\ -u_5\\ -u_6\end{array}\right]-\left[\begin{array}{cccc}1& -1& 0& 0\\ 0& 1& -1& 0\\ 0& 0& 1& -1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_0\\ x_1\\ x_2\\ x_3\end{array}\right]=\left[\begin{array}{c}{e}_{u,1}\\ e_{u,2}\\ e_{u,3}\\ z_{u,1}\\ z_{u,2}\\ z_{u,3}\end{array}\right]$$

那么：
$$H=\left[\begin{array}{cccc}1& -1& 0& 0\\ 0& 1& -1& 0\\ 0& 0& 1& -1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
且 $\Sigma =diag({\mathbf{Q}}_1,{\mathbf{Q}}_2,{\mathbf{Q}}_3,{\mathbf{R}}_1,{\mathbf{R}}_2,{\mathbf{R}}_3)$.
整个问题可以写成：
$$x_{map}^{\ast }=argmin{e}^T{\Sigma }^{-1}e$$
之后将会看到，该问题有唯一的解：
$$x_{map}^{\ast }=(H^T{\Sigma }^{-1}H{)}^{-1}{H}^T{\Sigma }^{-1}y$$