非线性优化
非线性优化
1.经典SLAM模型
方程中位姿可由变换矩阵来描述，然后用李代数来优化，观测方程由相机成像模型给出，内参为出厂固定，外参由相机位姿确定。由于噪声的存在，再高精度的相机，运动方程和状态方程也是近似的，此时通过优化处理噪声数据，由表层逐渐深入到图优化本质。
2.最大后验与最大似然
假设两个噪声项wk和vk满足零均值高斯分布，由此引出状态估计问题：通过带噪声的数据z和u，推断位姿x和地图y以及他们的概率分布。历史上广泛应用的是滤波器，尤其是扩展卡尔曼滤波的方法，但是近些年来，非线性优化方法，普遍比滤波器方法更有效，成为了视觉SLAM的主流方法。
在非线性优化中，把所有待估计的变量放在一个状态变量中，对于机器人状态的估计，就是求已知输入数据u和观测数据z的条件下，计算状态x的条件分布P(x|z,u),只有一张张图像时，则只考虑观测方程的数据，相当于估计P(x|z)的条件概率分布。利用贝叶斯法则：
直接求后验分布较难，转而求一个状态最优估计，使得该状态下，后验概率最大化，得出一个较好估计：
分母部分由于和待估计的状态x无关，可以忽略，即：求解最大后验概率相当于最大似然和先验的乘积。若出现不知道机器人位置的情况，此时就没有了先验，此时x的最大似然估计为：
似然是指，在现在的位姿下，可能产生怎么样的观测数据，由于观测数据的已知，直接的理解最大似然估计，就是在什么状态下，最可能产生现在观测到的数据。
3.最小二乘法
通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。
假设噪声项vk服从参数为（0，Qk,j）的正态分布，则观测数据的条件概率为：
使用最小化负对数的方法，求一个高斯分布的最大似然，任意高维高斯分布
，此概率密度函数展开为：
取负对数，则变为：
对原分布求最大化相当于对负对数求最小化，第一项和x无关，略去，代入SLAM观测模型，可得：
该式等价于最小化噪声项的平方，定义数据与估计值之间的误差为：
求该误差的平方和：
它的最优解等价于状态的自打似然估计。此为总体意义下的最小二乘问题。
SLAM中的最小二乘问题具有一些特定的结构：
整个问题的目标函数由许多个误差的（加权的）平方和组成。但每个误差项都是简单的，仅与一两个状态变量有关。每个误差项是一个小规模的约束，把这个误差项的小雅可比矩阵块放到整体的雅可比矩阵中。称每个误差项对应的优化变量为参数块。
整体误差由很多小型误差项之和组成的问题，其增量方程的求解会具有一定的稀疏性，使得它们在大规模时亦可求解。
使用李代数表示，则该问题是无约束的最小二乘问题。
使用了平方形式（二范数）度量误差，它是直观的，相当于欧氏空间中距离的平方。
对于函数形式简单的最小二乘问题，直接使用求导模型，解出导数为零处的值即为极值。对于不方便求解的最小二乘问题，用迭代的方式，从一个初始值出发，不断地更新当前的优化变量，使目标函数下降。
这使得求解导数的问题，变成了一个不断寻找梯度并下降的过程，直到算法收敛。此过程中，只要找到迭代点的梯度方向即可，无需找导数为零的点。
4.一阶和二阶梯度法（牛顿法）
增量如何确定，最直观的方式是，将目标函数在x附近泰勒展开： 
J是雅各比矩阵，H是海塞矩阵，只保留一阶梯度，增量的方向为：
直观意义为，沿着反向梯度方向前进，计算该方向的一个步长λ，求最快下降方式，此为最速下降法。
二阶梯度法即保留二阶梯度信息，增量方程为：
求右侧等式关于德尔塔x的并另为零，得到增量的解：
两种方法的问题：
最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。
牛顿法则需要计算目标函数的H矩阵，这在问题规模较大时非常困难，通常倾向于避免H的计算。
5.高斯牛顿法
高斯牛顿法是最优化算法中最简单的方法之一，思想是将fx进行一阶泰勒展开：
这里的J(x)是f(x)关于x的导数，是一个m×n矩阵，是为了寻找下降矢量
，使得最小，为求，需解决一个线性的最小二乘问题：
将上述目标函数对求导，令导数为零，具体推导如下：
求导，令为零：
此方程称为增量方程，即高斯牛顿方程或者正规方程左边定义为H，右边定义为g，上式变为：
对比牛顿法，高斯牛顿法用作为牛顿法中海塞矩阵的计算，省略了海塞矩阵的计算，求解增量方程是整个优化问题的核心，具体步骤如下：
高斯-牛顿法的问题:
在使用时，可能出现为奇异矩阵或者病态 (ill-condition) 的情况，此时增量的稳定性较差，导致算法不收敛。
假设H非奇异也非病态，如果求出来的步长∆x太大，也会导致局部近似不够准确，甚至都无法保证它的迭代收敛。
6.
列文伯格-马夸尔特方法
此方法在一定程度上修正了上述问题，一般认为比牛顿高斯方法更加健壮，但收敛速度比高斯牛顿法慢，被称为：阻尼牛顿法，但在SLAM中大量应用。
由于高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近由较好的近似效果，所以使用信赖域方法：给∆x添加一个信赖区域，不能让它太大而使得近似不准确。信赖域里，我们认为近似是有效的。
如何确定信赖域的范围：根据近似模型和实际函数之间的差异来确定，差异大，就缩小范围，差异小，就增大范围，使用下公式判断泰勒近似是否够好：
如果ρ接近于1，则近似是好的。如果ρ太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果ρ比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。
接下来给出一个改良版的非线性优化框架：
这里近似范围扩大的倍数和阈值都是经验值，可以替换，式6.24中，列文伯格提出的优化方法是：将D取成单位矩阵I，相当于把∆x约束在一个球中，后来马夸尔特提出将D取成非负数对角矩阵。
用Lagrange算子将它转化为一个无约束优化问题：
展开，计算增量的线性方程：
D=I：
当参数λ较小时，H占主要地位，这说明二次近似模型在该范围内是比较好的，L-M方法更接近于G-N法。另一方面，当λ比较大时，λI占据主要地位， L-M 更接近于一阶梯度下降法（即最速下降）。
L-M的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量∆x。
