三维重建中的非线性优化学习笔记

参考资料：slam十四讲第6讲
理解并实践

最终需要回答是否完成了这三个目标
这样主要讨论的问题总结来说就是解决在有噪声的情况下如何进行准确的状态估计

6.1 状态估计问题

6.1.1 最大后验与最大似然

经典SLAM模型：

运动方程先不讨论，观测方程可以具体化为：

这个方程在上一章有仔细讨论，这里先不展开

假设噪声服从高斯分布

有两种方法可以进行状态估计：一种是EKF，它只关心当前时刻的状态估计，而对之前的状态不多考虑；另外一种是非线性优化方法，它使用所有时刻采集到的数据进行状态估计，并被证明优于滤波的方法。以下我们讨论的是非线性优化的方法。

有状态向量：

现在需要求解的就是

在没有测量运动的传感器时，控制量u是未知的，因此对求解的问题进行简化，在利用贝叶斯公式：

直接求概率分布是困难的，我们可以进一步简化成求后验概率最大化情况下的状态向量，也就是最大后验概率

然后先验概率我们不知道，因此进一步简化模型：

这就是最大似然估计

6.1.2 最小二乘的引出

最小二乘的引出其实就是为了解决以上的最大似然估计的问题。

回顾观测模型：

可以求出条件概率：

是一个高斯分布，以下就是求高斯部分的最大似然。
首先是N维高斯分布的概率分布：

取负对数：

求原分布的最大化相当于求负对数的最小化，又因为右边第一项与x无关，因此就是求

相当于最小化噪声项的平方。因此我们定义误差：

然后求误差的平方之和：

这就等价于刚才提到的最大似然估计，所以现在我们的问题是研究什么情况下J可以最小，这实际上是一个最小二乘问题。

这个SLAM的最小二乘问题具有一些特定的结构：

• 整个目标函数有许多个简单的误差的平方和得到。每个误差项仅和一两个状态向量有关
• 它的增量方程有一定的稀疏性
• 如果使用李代数表示，则该问题是无约束的最小二乘问题
• 我们使用了平方形式度量误差，它相当于欧式空间中的距离的平方，但是这种范数的表示方式不是唯一的。

6.2 非线性最小二乘

首先考虑最简单的最小二乘问题：

如果对于简单的f来说，直接求导得到极值点再进行比较就可以得到最小值，进而求得x；对于不方便直接求导的最小二乘问题，我们可以使用迭代的方式：

那接下来的关键问题在于如果求解增量$\Delta$x

6.2.1 一阶和二阶梯度法

最简单的方法是将目标函数在x附近进行泰勒展开


如果保留一阶项，那么增量的方向为：

它的直观意义非常简单，只要我们沿着反向梯度的方向前进即可（函数值下降最快的方向），这种方法也被称为最速下降法。缺点是过于贪心，容易走出锯齿路线，反而增加迭代次数。

另外一方面，如果保留二阶梯度信息，那么增量方程为

求右侧等式关于$\Delta$x的导数并令他为0，就得到增量的解：

这称为牛顿法，缺点是计算H比较复杂。接下来介绍两种更加实用的方法。

6.2.2 Gauss-Newton

它的思想是对f(x)进行一阶泰勒展开，注意不是目标函数f(x)^2


现在我们要做的是求解$\Delta$x使得||f(x+$\Delta$x)||^2达到最小，也就是：

我们先将目标函数展开：

再让上式对$\Delta$x求导并令其等于0，化简后有：

上式也被称为高斯牛顿方程或者正规方程。用H和g代替可以写成如下形式：

Gauss-Newton法是对牛顿法中的Hessian矩阵用J^TJ做了近似

所以整个Gauss-Newton法的步骤如下：

Gauss-Newton法存在的问题：原则上，我们使用的近似H矩阵要求是可逆的，但是在实际数据计算得到的J^TJ却只有半正定性，也就是J^TJ有可能出现奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛。更加严重的是，就算我们假设H非奇异也非病态，如果我们要求的$\Delta$x太大，也会导致我们采用的局部近似不够准确，这样一来我们甚至无法保证它的迭代收敛。

6.2.3 Levenberg-Marquadt

LM方法在一定程度上修复了高斯牛顿法的缺点，虽然收敛速度更慢，它也被称为阻尼牛顿法。针对高斯牛顿法中近似二阶泰勒展开只能在展开点附近有比较好的近似效果。我们可以给$\Delta$x添加一个可信赖区域。在信赖区域内我们认为近似是有效的，出了这个区域，我们认为近似可能会出问题。

下面就是如何确定可信赖区域

所以LM算法的步骤如下：

我们可以把上面的(6.24)转换成无约束的优化问题：

Hessian矩阵用J^TJ做了近似，可以看到增量方程相比于高斯牛顿法增加了一项，如果D=I,则：

可以看到当λ比较小时，H占主导地位，说明二次近似模型在该范围内比较好，L-M法接近于G-N法；当λ比较大时，λI占主导地位，L-M法更接近与一阶梯度下降法。

6.2.4 小结

以上可以发现，初始化值对最终的优化结果是有很大影响的；
求解增量方程怎么求？直接算逆计算量太大，常用QR分解、Cholesky分解等方法，而且slam中这个矩阵往往有特定的稀疏形式，可以进一步降低计算量。