SLAM重点摘要4:非线性优化方法

关键词：最小二乘法，增量方程

状态估计问题的表述：
在之前我们已经讲过了经典的SLAM数学模型，由运动方程和观测方程两者所组成。如下：

1. xk = f(xk-1, uk) + wk
2. zk,j = h(yj, xk) + vk,j

方程1被称为运动方程，描述相机的运动，方程2被称为观测方程，描述特征点的观测模型。

而在相机模型中，依据内参外参矩阵，有如下方程：
Szk,j = K exp(&^)yj
K为相机内参，exp(&^)为相机外参的李代数，yj为空间点在世界坐标系下的三维位置坐标，K对于相机为固定值（当然此时我们还没有考虑相关的畸变模型，在之后我们会考虑畸变模型构建一个更复杂的方程）

此处假设w和v都是均值为0的高斯噪声：
wk ~ N(0, Rk)
vk,j ~ N(0,Qk,j)

而我们知道，SLAM问题是一个已知u和z，求相机每一步的姿态x和特征点空间坐标y的问题，所以从概率学的角度，在非线性优化中，我们把所有待估计的变量放在一个“状态变量”中：
x = {x1, … , xN, y1,…,yM}

对机器人状态的估计，就是已知输入数据u和观测数据z的条件下，求计算状态x的条件概率分布： P(x|z,u)
当没有测量运动的传感器，而只有图像时，比如ORB-SLAM,可以简化为：P(x|z)
依据贝叶斯法则，我们有：
P(x|z) = P(z|x)P(x)/P(z)
我们不知道图片分布的概率，也不知道P(x)，此时就没有了先验概率，因此我们可以求解x的最大似然估计（Maximize Likelihood Estination, MLE）：
x*MLE = arg max P(z|x)
我们想要P(x|z)最大，即我们要求出在现有输入下，变量x最可能的取值，由贝叶斯公式，我们将其转换为了求P(z|x)最大值的问题，即变为了在什么情况的x下，取z的概率最大！

[–当然这里面的假设太多了，我不太理解为啥这样做可以，即这样的省略正确–]

由于观测方程：
zk,j = h(yj, xk) + vk,j
【当然，现在看来，从P(x|z)转化为P(z|x)是为了顺应之前的相机模型方程，由三维点投影到相片是可以计算的，但是从相片反投影回三维点，只能计算反投影到标定平面上的那个点，计算不出来在三维平面的那个点，这需要更复杂的计算。】
h表示一系列的函数运算，v为高斯分布的变量，根据概率论的知识，有：
vk,j ~ N(0,Qk,j)
P(zj,k | xk, yj) = N(h(yj, xk), Qk,j)
这里相当于xk,yj为常量了！
这仍然是一个高斯分布，为了计算使其最大化的xk,yj，我们往往通过最小化负对数的方式来求解。

高维高斯分布模型：
x ~ N(u,∑)，概率密度展开形式为：
P(x) = exp(-1/2 (x-u)T ∑-1 (x - u))/sqrt(2pai)N * det(∑))
取其负对数，去掉无关项，只保留未知项，得到：
x* = arg min( (zk,j - h(xk, yj))T Qk,j-1 (zk,j - h(xk,yj)) )

而这样其实就把可能相关的多元变量的最小二乘法的雏形表现出来了
意义：最小化噪声项（误差）在∑范数下的平方
因此，我们定义数据与估计值之间的误差：
ev,k = xk - f(xk-1, vk)
ey,j,k = zk,j - h(xk,yj)

并求该误差的平方和，把两式的误差分别加起来（当然，这里也要涉及到我们很头疼的一个问题，那就是权重问题，两式是独立的，因此我们将其直接加起来，这一步是我最怀疑的，也是我感觉最没有依据的一步）
当然了，同一个式子里面的权值，有噪声为依据进行加权，但问题也来了，你如何知道每一步的噪声的分布呢？难道每次都去取点计算，逼近噪音吗？

普遍的非线性最小二乘的优化问题：
如： min(x) 1/2 ||f(x)||^2
其中，自变量x属于 R^n，f是任意的非线性函数，我们设它有m纬，形成一个n -> m的空间投影，解决这样一个优化问题的步骤如下：

1. 给定某个初始值x0
2. 对于第k次迭代，寻找一个增量delta xk，使得 ||f(xk + delta xk)||^2达到极小值
3. 若delta xk 足够小，则停止
4. 否则，令 xk+1 = xk + delta xk，返回第二步

（梯度下降法）

多元函数的泰勒展开：
只展示前两项：
||f(x + delta x)||^2 ~~ ||f(x)||^2 + J(x)delta x + 1/2 delta x T H delta x
J 是 ||f(x)||^2对于x的一阶导数（雅可比矩阵）,H是二阶导数（海塞[Hessian]矩阵）

1. 如果只保留一阶梯度，那么这就是一条直线，我们视增量解为
   delta x* = -JT(x)
   意义就是沿着反向梯度方向前进即可（在我看来就类似于一条曲线的切线，沿着下降的方式前行，部分情况下还可以增加一个布长 lamda）
2. 如果保留二阶梯度信息，使得导数为0，对泰勒展开对于delta x求解，那么增量的解为：
   H delta x = -JT
   但是该方法还要求解H（海塞）矩阵，这样计算量较大，计算多元函数的二阶导数，J(x)为一阶矩阵

由于我们通常想要避免H矩阵的计算，所以我们在实际中有两种更为实用的方法：高斯牛顿法和列文伯格-马夸尔特方法。

高斯牛顿法：
先讲f(x)进行一阶泰勒展开，然后再将其平方，从而用两个一次导表示出二次导数，简化计算量。
f(x+delta x) = f(x) + J(x)delta x
对 1/2||f(x + delta x)||^2 进行delta x求导之后，我们简化得到：
J(x)TJ(x)delta x = -J(x)Tf(x)
将左侧定义为H,右侧定义为g，那么上式变为：
Hdelta x = g
这样计算更简单

列文伯格-马夸尔特方法：
在高斯牛顿法的基础上进行修改，给delta x加一个信赖区间(Trust Region)：把增量解限定在一个椭圆的球里面
最初始：
min 1/2||f(xk) + J(xk)delta xk ||^2 , s.t. || Ddelta xk||^2<= u
u是给定的信赖区域半径，会不断的更新

但是在SLAM十四讲中这一块并不清楚，如何将定义域转化到无约束优化问题里面，以及如何将矩阵简化为I，都不清不楚！
这里我们简要记为：
(H + lamda I)delta x = g
这里的H和g和高斯牛顿法是一样的。
有助于解决H矩阵奇异值的问题。
当lamda较小的时候，偏向于高斯法，当lamda比较大的时候，偏向于一阶梯度法。

当然，如何求解delta x是个很重要的问题，直接对系数矩阵求逆是很大的计算量，多亏于系数矩阵的系数性质，在下一章节我们将会讲到。
下一章节： BA优化