ORB-SLAM2 ---- 非线性优化在SLAM中的应用（一）

一、为什么要讲非线性优化

相信大家在学习一段时间SLAM后，会发现两个问题。第一个是代码能看懂，但是不知道为什么这样做（特别是优化部分）。第二个问题就是数学问题，SLAM最难最迷人的部分在于它很多原理归结到了数学问题上。在看优化的时候，因为g2o库的原因，很多数学公式的内在逻辑没有在代码中展现出来，这几篇文章将从最开始的运动模型和观测模型，一步步的推导到优化中用到的列文伯格-马夸尔方法。将非线性优化放在这里将是因为大家已经有了一定的基础，理解起来会更加的容易。

二、运动模型和观测模型

上述公式是最简单的运动模型和观测模型，x表示相机的位姿，y表示路标的坐标，z表示路标在相机中的像素坐标。我们将其与ORB-SLAM2联系起来，这里的x可以表示每一帧相机的位姿，y表示地图点的世界坐标，z表示地图点在某一帧中的投影像素坐标（特征点的像素坐标）。w和v分别表示运动模型和观测模型中的噪声。通常认为噪声符合高斯分布，如上图所示。这两个模型相机位姿x和像素点坐标z看起来像是待求量，但是在学过一段时间SLAM后就知道，我们关注的是相机的位姿和地图点的时间坐标，也就是x和y。

三、最大似然估计

在上述的两个模型中讲到，我们要求的量是x和y，也就是在u和z的条件下求x和y，这里的u（在追踪线程的恒速跟踪模型中，跟踪速度就相当于u）和z（我们提取的特征点在某一帧中的去畸变坐标）是已知量。故我们要求的是在u和z的情况下x和y的值。这看起来是一个直接计算的问题，但是其中要噪声的干扰使得我们无法求得一个准确得x和y，我们能做的是找出尽可能小得误差下相机得位姿和地图点坐标（x和y）。这个问题就转换成一个概率问题：

我们利用贝叶斯公式将其展开：
可以看出分母与x和y无关，可以将看作比例系数，正比于似然乘以先验。求出x、y条件下z、u的最大概率可以反推，z、u条件下x、y的最大概率，而由于SLAM中没有先验，即求最大似然。

四、SLAM中最小二乘的应用

我们这里以观测模型为例子，讲解单个误差的构成。由上述的观测模型和噪声的高斯特性可以推出下面的公式，我用比较抽象的简笔画简述了原因：


高斯模公式如下图所示，让其取负对数更加容易求解后面的问题


由于对数函数是单调增加的函数，取负对数就是单调减少，故要求最大似然，就要求负对数函数的最小值对应的x，这个公式的第一项与x无关也可以省略，我们将观测模型的高斯表达式代入，即我们要求的是：

我们一开始是以观察模型的一个误差为例，其他误差也是相同的道理，我们将所有的误差累加到一起，求最小值，所得到的x和y才是我们最终需要的x和y。因为我们最终的目的是相机的位姿和地图点的坐标准确，所以不因该求单个误差最小时的x和y，而是所有误差加起来最小时的x和y，即：

公式中的第一项时累积运动误差，第二项为累积观测误差。我们从公式中可以看出，误差项越多，单个误差对相机位姿个地图点坐标的影响就越小，这就是我们在学习优化时怎加约束的原因。

五、总结

本次讲解的内容文字可能不太直观难懂，我自己花了两页纸的空间以流程的形式一步步的进行了推导，能更直观看出每一个步骤直接的因果关系以及推到过程。