SLAM基础——如何从最大后验概率到最小二乘（应用篇）
理论篇回顾
总结一下《理论篇》的推理过程：
- SLAM后端优化的任务是最大似然函数，也就是在已知观测的情况下通过最大化似然函数确定最有可能的机器人位姿
- 最大似然在高斯分布的情况下最终可以转换成一个二次型，具体含义是最小化观测和模型的误差（经过信息矩阵也就是协方差倒数的加权）
用公式可以写为：
\[ \begin{equation} \begin{split} \hat{x}_{MLE} &= \underset{x}{\operatorname{argmin}} \big(z-h(x)\big)^T\Sigma^{-1}\big(z-h(x)\big)\\ &= \underset{x}{\operatorname{argmin}}||z - h(x)||^2_{\Sigma^{-1}}\\ &= \underset{x}{\operatorname{argmin}}||e_z||^2_{\Sigma^{-1}} \end{split} \end{equation} \tag{1} \]
最小二乘的标准形式
在前文的公式中，向量的符号没有加粗，且推导的过程没有用正式的符号，这里先对前面的结果做更标准化的复述。
令机器人每一时刻的位姿为\(\boldsymbol{x}_i, i\in [0,1,2,...)\)，每一时刻的观测为\(\boldsymbol{z}_i,i\in[0,1,2,...)\)，记机器人历史轨迹的位姿集合为\(\boldsymbol{X},\boldsymbol{x}_i\in\boldsymbol{X}\)，则公式（1）可以写为：
\[ \begin{split} \hat{\boldsymbol{X}}_{MLE} &= \underset{\boldsymbol{x}_i}{\operatorname{argmin}}\sum_{i}\big(\boldsymbol{z}_i - \boldsymbol{h}(\boldsymbol{x}_i)\big)^T\boldsymbol{\Sigma}_i^{-1}\big(\boldsymbol{z}_i - \boldsymbol{h}(\boldsymbol{x}_i)\big)\\ &=\underset{\boldsymbol{x}_i}{\operatorname{argmin}}\sum_{i}\boldsymbol{e}_i^T\boldsymbol{\Sigma}_i^{-1}\boldsymbol{e}_i \end{split}\tag{2} \]
上面的形式又叫加权的平方误差（Weighted Sum of Squared Error），权重系数在这里是\(\boldsymbol{\Sigma}_i^-1\)，也就是协方差的倒数，直观含义就是对于不确定的观测（方差较大）赋予较低的权重。我们可以对\(\boldsymbol{e}_i\)提前乘以一个系数\(L_i\)，\(L_i\)满足\(\boldsymbol{\Sigma}_i^{-1}=\boldsymbol{L}_i\boldsymbol{L}_i^T\)，并记\(\bar{\boldsymbol{e}}_i=\boldsymbol{L}_i\boldsymbol{e}_i\)，则
\[ \begin{split} \hat{\boldsymbol{X}}_{MLE}&=\underset{\boldsymbol{x}_i}{\operatorname{argmin}}\sum_{i}||\bar{\boldsymbol{e}}_i||^2 \end{split}\tag{3} \]
公式（3）称为最小二乘的标准形式。
不过要注意的是，在SLAM中一些优化问题中比如BA,是直接构造最小二乘问题，同时也没有加入权重。这并不影响我们的理解。最小二乘问题从直观上就是让误差最小化。
BundleAdjustment
我觉得在这里加入BA(BundleAdjustment)问题的讲解可以加深对这部分的印象和理解。BA又叫光束法平差，或者捆绑调整。Bundle的意思是同时对相机位姿和关键点进行优化。
PS：这部分其实《视觉SLAM十四讲》第九讲讲得很好，也可以直接参考十四讲的内容。这里主要和前文的公式建立联系，让这一部分的内容更为完整。
假设机器人移动经过三个点，相机的位姿分别为\(\boldsymbol{x}_1,\boldsymbol{x}_2, \boldsymbol{x}_3\)，空间中有三个路标点\(\boldsymbol{p}_1,\boldsymbol{p}_2,\boldsymbol{p}_3\)
定义相机投影方程\(\pi(\cdot)\)，相机投影方程将世界坐标系下的点转换到相机坐标系然后投影到像素坐标系：
\[ \pi(\boldsymbol{p}) = K\big(R\boldsymbol{p}+\boldsymbol{t}\big) \]
其中，\(K\)是相机内参，\(R,\boldsymbol{t}\)是相机位姿
关键点可以直接通过图像计算得到，也就是观测\(\boldsymbol{z}_i := [u_i,v_i]^T\)
将所有时刻的位姿以及每个位姿能够观测到的特征点都累加起来，并用标准的二次平方损失函数表达，则整体的代价函数为
\[ \sum_{i=1}^{M}\sum_{j=1}^{N}||\boldsymbol{e}_{ij}||^2=\sum_{i=1}^{M}\sum_{j=1}^{N}||\boldsymbol{z}_i-\pi(\boldsymbol{p}_j)||^2 \]
将相机位姿\(\boldsymbol{x}_i\)和路标（landmark）点\(\boldsymbol{p}_j\)共同当作变量进行优化，得到优化方程如下：
\[ \begin{split} \hat{\boldsymbol{X}},\hat{\boldsymbol{P}} &= \underset{\boldsymbol{x}_i, \boldsymbol{p}_j}{\operatorname{argmin}}\sum_{i=1}^{M}\sum_{j=1}^{N}v_{ij}||\boldsymbol{z}_i-\pi(\boldsymbol{p}_j)||^2 \end{split}\tag{3} \]
其中
- \(M\)是位姿点的数量，\(N\)是路标点的数量
- \(v_{ij}\)是一个二元变量，当点\(\boldsymbol{p}_j\)能够被位姿点\(\boldsymbol{x}_i\)观察到时，\(v_{ij}=1\)，否则\(v_{ij}=0\)
至于要如何求解上面的最小二乘公式，就是非线性优化了，要讲清楚可能要单开一篇文章。但是从上一篇还有这一篇的论述，读者应该要清晰地建立从MAP问题到最小二乘问题的推演过程，这样也才能够更好地理解论文里频繁出现的MAP
鲁棒核函数
让我们重新看一下上述公式（3），求解这样的一个二次型一般需要用非线性优化方法，优化过程中会涉及到对这个二次型求偏导。这样就会出现一个问题，如果\(\boldsymbol{z}_i\)与\(\boldsymbol{p}_j\)的匹配关系出现错误，或者\(\boldsymbol{z}_i\)计算错误，那么二次型就会非常大。这样在优化过程中，导数会被错误项占据主要地位，这样更新的时候就往错误的方向进行更新。
PS:在上述描述的BA问题中，\(\boldsymbol{z}_i\)通常是对图像进行特征点检测得出特征点的位置和描述子，\(\boldsymbol{p}_j\)是地图中点，两者的匹配关系通常可以通过特征点匹配的方式得到。
为了解决上面的问题，我们把二次函数替换为其他函数，使得在误差较大的地方函数值不至于过大导致导数过大。这些函数就叫鲁棒核函数（Robust Kernel）。
Huber核函数
一种较为常见的核函数叫做Huber核函数，在很多论文中都使用该函数，其具有如下形式
\[ H(e) = \begin{cases} \frac{1}{2}e^2,&||e||\leq \delta\\ \delta\left(||e||-\frac{1}{2}\delta\right),&else \end{cases}\tag{4} \]
Huber核函数与二次函数的函数曲线如下图
核函数的特点和其他核函数
从上面的描述中我们其实不难得出鲁棒核函数应该具有的两个特点：
- 光滑。即处处可导
- 函数增长的速度要比二次函数慢
从Huber函数公式（4）我们可以看出当\(||e||=\delta\)的时候，上下两式结果是一样的，因此Huber函数明显是一个光滑的函数。
常见的其他鲁棒核函数还有Cauchy、Tukey核等，从网上找了一张各个核函数和二次函数的函数曲线对比图
一些思考
鲁棒核函数让我联想到了神经网络中的激活函数，SLAM中的图优化也让我觉得跟神经网络的梯度回传有点相似。神经网络的训练目标其实也是MAP问题（从训练集中估计出样本的分布，并使用该分布来预测测试集），而且很多的神经网络目标函数也是一个二次型。不过区别比较大的是，神经网络中大部分操作是线性操作（卷积、全连接），而SLAM中很多操作都是非线性的（旋转）。在图优化中的更新过程使用非线性优化方法（比如高斯牛顿法）也是需要在每一步更新过程中近似估计目标函数对于变量的偏导数，这点与神经网络的梯度回传是类似的。
既然三层全连接层的神经网络就可以拟合任意次的函数，那么是否有可能用神经网络来做SLAM的后端优化？