作者：Frank
时间：2016-07-31

不论是在Feature-based SLAM还是在Direct-Method SLAM中，为了求得当前帧和参考帧直接的相对相机运动(位姿)，都涉及到求解非线性优化问题。在Feature-Based SLAM中，我们可以在得到参考帧和当前帧的匹配对后利用opencv自带的solvePnp函数来求解位姿，而solvePnp中经典的是EPNP算法，算法中就涉及到利用Gauss-Newton迭代来求解最优化参数；而在Direct-Method SLAM中，算法直接根据图像像素匹配误差(photometric error)来构造优化函数来求其最优解。在SLAM的优化问题中常见的有Gauss-Newton迭代和Levenberg-Marquardt迭代，本节和下节将会分别就这两种优化方法展开讲述（主要是针对直接法中需要用到的优化解）。

1.Gauss-Newton优化

1.1.原理

Gauss-Newton迭代的基本思想是使用泰勒级数展开式去近似代替非线性回归模型，然后通过构造优化函数，并进行多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。
假设已知m个点：$(x_1,y_1),(x_2,y_2),…..,(x_m,y_m)$，函数原型为：$y=f(x, \beta)$，其中$\beta =(\beta_1,\beta_2,……..,\beta_n)$，(m不能小于n，因为函数个数需要大于未知参数个数才能得到方程的解)。构造残差平方和函数如下：
$$S=\sum_{i=0}^{m}r_i^2 \tag{1}$$
其中：
$$ r_i=y_i-f(x_i,\beta) for i=1,2,….,m. \tag{2}$$
最优化的目的是找到最优解$\beta$使得残差平方和最小。
要求其最小值，即$S$对$\beta$的偏导数等于0：
$${\partial S \over \partial \beta_j}=2\sum_{i}{r_i{\partial r_i \over \partial \beta_j}}=0 \ (j=1,2,……,n) \tag{3}$$
在非线性优化系统中，$\partial r_i \over \partial \beta_j$是变量和参数的函数，没有闭解。因此我们给定一个初始值，用迭代法逼近解：
$$ \beta_j \approx \beta_j^{k+1}=\beta_j^k+\Delta \beta_j \tag{3}$$
其中k是迭代次数，$\Delta \beta$是迭代矢量。
而每次迭代函数是线性的，在$\beta^k$处用泰勒级数展开：
$$f(x_i,\beta)\approx f(x_i,\beta^k)+\sum_j {\partial f(x_i,\beta^k) \over \partial \beta_j}(\beta_j-\beta_j^k) \approx f(x_i,\beta^k)+\sum_jJ_{ij}\Delta \beta_j\tag{5} $$
其中：J是已知矩阵，为了方便迭代令${\partial r_i \over \partial \beta _j}=-J_{ij}$。
此时残差表示为：
$$\Delta y_i=y_i-f(x_i,\beta^k) \tag{6}$$
$$r_i=y_i-f(x_i,\beta)=(y_i-f(x_i,\beta^k))+(f(x_i,\beta^k)-f(x_i,\beta))=\Delta y_i-\sum_{s=1}^{n}J_{is}\delta \beta_s \tag{7}$$
代入公式{3}有：
$$-2\sum_{i=1}^{m}J_{ij}(\Delta y_i-\sum _{s=1}^{n}J_{is}\Delta \beta_s)=0 \tag{8}$$
移项化简有：
$$\sum_{i=1}^{m} \sum_{s=1}^{n} J_{ij}J_{is} \Delta \beta_s=\sum_{i=1}^{m}J_{ij} \Delta y_i \ (j=1,….,n) \tag{9} $$
将其表示成矩阵形式有：
$$(J^TJ)\Delta \beta=J^T \Delta y \tag{10} $$
所以最终迭代公式为：
$$\beta^{(s+1)}=\beta^{(s)}+(J_f^TJ_f)^{-1}J_f^Tr(\beta^{(s)}) \tag{11} $$
其中，$J_f$是函数$f=(x,\beta)$对$\beta$的雅克比矩阵。

申明：本部分引用来源：最小二乘法–高斯牛顿迭代法

总结

Gauss-Newton法的介绍在这里就结束了，下一节将会讲述Levenberg-Marquardt迭代，在之后的博客中还会涉及到SLAM部分相关代码的编写和整理，谢谢！

转载请注明出处