SLAM非线性优化

经过前面的学习，我们的问题变为如何在有噪声的数据中进行准确的状态估计。

状态估计问题#

经典SLAM模型，由一个运动方程和一个观测方程构成：

$$\begin{cases} x_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j} \end{cases}$$

这里的$x_k$就是相机的位姿，可以由$SE(3)$（特殊欧氏群，欧氏变换组成的）来描述。观测方程由针孔模型给定。

假如在$x_k$处对路标$y_j$进行了一次观测，对应到图像上的像素位置为$z_{k,j}$，那么，观测方程可以表示成：

$$sz_{k,j}=K(R_ky_j+t_k)$$

在运动和观测方程，我们通常假设两个噪声项$w_k,v_{k,j}$满足零均值的高斯分布：

$$w_k\sim \Nu(0,R_k),v_k\sim \Nu(0,Q_{k,j})$$

其中$\Nu$表示高斯分布，0表示零均值，$R_k$,$Q_{k,j}$为协方差矩阵。

在这些噪声的影响下，我们希望通过带噪声的数据z和u推断位姿和地图y以及他们的概率分布（u是啥？)，这就是一个状态估计问题。
目前解决状态估计问题的思路：

• 增量/渐进的方式：滤波器
• 批量的方法

批量状态估计和最大后验估计#

这里我们讨论批量方法，考虑1到N的所有时刻，并假设M个路标点。定义所有时刻的机器人位姿和路标点坐标为

$$x=\{x_1,...,x_N\}, y=\{y_1,...,y_M\}$$

u表示所有时刻的输入，z表示所有时刻的观测数据。则就是求：

$$P(x,y|z,u)$$

特别的，当我们不知道控制输入，只有一张张图片时，即只考虑观测方程带来的数据时，可以转化为：

$$P(x,y|z)$$

此问题也称为SfM问题，即如何从许多图像中重建三维空间结构。
利用贝叶斯法则：

$$P(x,y|z,u)=\frac {P(z,u|x,y)P(x,y)}{P(z,u)}\propto P(z,u|x,y)P(x,y)$$

$P(x,y|z,u)$ 称作后验概率，$P(z|x)$称为似然，$P(x)$称为先验。
直接求后验是困难的，但是求一个状态的最优估计，使得在该状态下后验概率最大是可行的：

$$(x,y)^*_{MAP}=argmaxP(x,y|z,u)=argmax P(z,u|x,y)P(x,y)$$

$P(x,y)$与待估计的部分无关，所以可以忽略。
贝叶斯告诉我们最大后验概率等价于最大化似然和先验的乘积。但是我们也可以忽略机器人位姿或路标位置，此时就没有了先验。那么可以求解最大似然(Maximize Likelihood Estimation,MLE)

$$(x,y)^*_{MLE} = argmax P(z,u|x,y)$$

最大似然可以理解为，“在什么状态下，最可能产生现在观测到的数据”

非线性最小二乘#

$$min_xF(X)=\frac {1}{2}\lvert f(x) \rvert^2$$

f(x)为非线性函数，有些导函数可能形式复杂，我们可以使用迭代的方式来接近局部极值
1.给定初始值x_0
2.对第k次迭代，寻找增量$\triangle x_k$，使得$\lvert f(x_k+\triangle x_k) \rvert^2$达到极小值
3.若$\delta x_k$ 足够小，则停止
4.否则，令$x_{k+1}=x_k+\triangle x_k$返回第2步

此时，问题从求解导函数为0转变到不断寻找下降增量$\delta x_k$ 的问题。

一阶和二阶梯度法#

泰勒展开：

$$F(x_k+\triangle x_k)\approx F(x_k)+J(x_k)^T \triangle x_k+\frac{1}{2}\triangle x_k^TH(x_k)\triangle x_k$$

其中$J(x_k)$是$F(x)$关于x的一阶导数（也叫梯度、雅可比矩阵）；H则是二阶导数（海塞矩阵）
一阶梯度：
方向：$\triangle x^*=-J(x_k)$
再指定步长，完成下降。

二阶梯度/牛顿法：
$\triangle x^*=argmin(F(x)+J(x)^T \triangle x+\frac{1}{2}\triangle x^TH(x)\triangle x)$
右侧对$\triangle x$求导，并令为0：

$$J+H\triangle x = 0$$

则， $$H\triangle x=-J$$

高斯牛顿法#

它的思想是将$f(x)$进行一阶泰勒展开而不是$F(x)$:

$$f(x_k+\triangle x_k)\approx f(x_k)+J(x_k)^T \triangle x_k$$

求解问题变为：

$$\triangle x^*=argmin _{\triangle x}\frac {1}{2}\lvert f(x)+J(x)^T\triangle x\rvert ^2$$

$$\frac {1}{2}\lvert {f(x)+J(x)^T\triangle x}\rvert ^2=\frac {1}{2}(\lvert f(x) \rvert ^2+2f(x)J(x)^T\triangle x+\triangle x^TJ(x)J(x)^T\triangle x$$

右侧求导， $$J(x)f(x)+J(x)J^T(x)\triangle x=0$$
则，可得 高斯-牛顿方程：

$$J(x)J^T(x)\triangle x=-J(x)f(x)$$

左边的定义为$H$右边的定义为$g$：
$H\triangle x=g$
对比牛顿法，其实高斯-牛顿法运用$JJ^T$作为牛顿法中二阶Hessian矩阵的近似，从而省略了H的估计。
算法流程：
1.给定初始值$x_0$
2.对于第k次迭代，求出当前的雅可比矩阵$J(x_k)$和误差$f(x_k)$
3.求解增量方程：$H\triangle x_k =g$
4.若\triangle x_k足够小则停止。否则，令$x_{k+1}=x_k+\triangle x_k$，返回第2步

列文伯格-马夸尔特方法#

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果。我们可以增加信赖区间，在区间范围内可以认为二阶近似是有效的。

$$\rho=\frac{f(x+\triangle x)-f(x)}{J(x)^T\triangle x}$$

算法流程：
1.给定初始值x_0，以及初始优化半径$\mu$;
2.对于第k次迭代，在高斯牛顿的基础上加上信赖区域：

$$min\frac{1}{2}\lvert f(x)+J(x_k)^T\triangle x \rvert ^2，s.t. \lvert D \triangle x_k \rvert ^2 \leq \mu$$

3.计算$\rho$
4.若$\rho \ge \frac {3}{4}$，则设置$\mu = 2\mu$
5.若$\rho \le \frac {1}{4}$，则设置$\mu = 0.5\mu$
6.如果\rho大于某阈值，则认为近似可行。令x_k+1=x_k+\triangle x_k;
7.判断算法是否收敛。如不收敛返回第2步，否则结束。

D、$\mu$求解：

$$lagelangri(\triangle x,\lambda)=\frac{1}{2}\lvert f(x)+J(x_k)^T\triangle x \rvert ^2+\frac{\lambda}{2}(\lvert D\triangle x_k \rvert ^2 - \mu)$$

右侧等于0 可得：

$$(H+\lambda D^TD)\triangle x_k=g$$

简化为

$$(H+\lambda I)\triangle x_k=g$$

可以理解为，利用参数$\lambda$控制两种方法的比例。
$\lambda$ 较小时，$H$ 占主要位置。列文伯格-马夸尔特方法更接近于高斯牛顿法。
$\lambda$ 较大时，$\lambda I$ 占主要位置，列文伯格-马夸尔特方法更接近于梯度下降法。