ORB-SLAM3学习：第一章--非线性最小二乘法

ORB-SLAM3学习

第零章–研究综述
第一章–非线性最小二乘法

---

---

前言

视觉SLAM中后端优化使用了诸多算法，非线性最小二乘法作为滤波算法的继承者，同时也是优化诸多算法中较为基础和简单的算法，对它的深入学习无疑是极为重要的。
参考视频资料：
1.最小二乘法简述 【「珂学原理」No.94「最小二乘估计了什么」】
https://www.bilibili.com/video/BV1sb411e79E/?share_source=copy_web&vd_source=9cb1868d1f25bd19d49d293bfbe568ac
2.高斯-牛顿公式推导【最优化算法之高斯牛顿法】
https://www.bilibili.com/video/BV1zE41177WB/?share_source=copy_web&vd_source=9cb1868d1f25bd19d49d293bfbe568ac

---

一、非线性最小二乘法在V-SLAM算法中的应用和简单理解

1.1 在V-SLAM中的简单说明

     ~~~~      简单来说，属于一种优化算法，用于后端优化，优化过程中只有来自前端处理好的数据（orb特征处理的结果），不关心前端数据来自哪些传感器。具体来说，在slam十四讲中，最小二乘法用于状态估计，看书的时候就很在意这个状态是什么。举例来说，机器人在运动时，我们可以用运动方程去描述机器人的状态，对于观测点（观察到的图像点）我们可以用观测方程去描述它，那么问题来了，机器人和观测点的关系怎么去描述，这也就是状态问题（ps:并不是最小二乘法要解决的问题，有公式可以解决，最小二乘法是优化这个函数）。

1.2 最小二乘法的简单引入

     ~~~~      接下来，让我们来达成一个共识：任何系统都会产生误差。所谓优化就是尽可能的接近真实值，将误差考虑进问题中，做状态估计的时候直接加入一个误差项，通过最小二乘法是误差项达到最小。
     ~~~~      我们从最经典的“尺子测量”问题来体会一下什么叫“优化”，人永远无法用尺子测得一个物体的长度，首先，测量时会出现各种各样的误差（看错了、没对好），其次，小数点后精确多少位也是无法测出的。我们测量了四次，次次不一样，完了，这个砖头没长度了。

     ~~~~      接下来我们引入最小二乘法，首先将问题抽象化。砖头测量抽象起来就是：$$Ax=b$$
     ~~~~      A是测量矩阵，x是砖块的长度，b是测量的结果，我们找不到一个完美的x来满足所有的等式，退而求其次吧，找到一个尽可能解释所有方程的x。把这个等式展开
$$\begin{gathered} a_{1}1x_1+a_{1}2x_2+\dots +a_{1}n{x}_n=b_1 \\ {a}_{2}1x_1+a_{2}2x_2+\dots +a_{2}n{x}_n=b_2 \\ ⋮ \\ {a}_{m}1x_1+a_{m}2x_2+\dots +a_{m}n{x}_n=b_m \end{gathered}$$
     ~~~~      那么我们如何找到那个“万金油”x呢，让所有的等式都基本满足呗，引入代价函数：
$$\begin{array}{rl}J(\mathbf{x})=& (b_1-(a_{1}1x_1+\dots +a_{1}n{x}_n){)}^2+\\ & (b_2-(a_{2}1x_1+\dots +a_{2}n{x}_n){)}^2+\\ & ⋮\\ & (b_m-(a_{m}1x_1+\dots +a_{m}n{x}_n){)}^2\end{array}$$
     ~~~~      什么叫代价函数呢，就是偏差的具象表现，优化就是要把这部分给尽量减小，找到那个x让J(x)最小。让一个函数值最小，求导呗，令其导数为零，x就出来了。
$$\left[\begin{array}{c}1\\ 1\\ 1\\ 1\end{array}\right]x=\left[\begin{array}{c}24.11\\ 24.05\\ 24.13\\ 24.12\end{array}\right]$$
     ~~~~      代入公式：
$$J(\mathbf{x})=(24.11-x{)}^2+(24.05-x{)}^2+(24.13-x{)}^2+(24.12-x{)}^2$$
     ~~~~      求导：
$$\frac{dJ}{x}=-2(24.11-x)-2(24.05-x)-2(24.13-x)-2(24.12-x)=0$$
     ~~~~      解得：
$$x=\frac{24.11+24.05+24.13+24.12}{4}$$
     ~~~~      这不就是我们常说的“测不准多测几次求平均值嘛”，当然这是最最最简单的一个例子。总结下来两点：1.求出代价函数。2.让代价函数最小化。至此，我们很浅显的了解了最小二乘法的基本思想，当然，这是线性的例子，很好理解，我们接下来要解决的问题是非线性的，主题思想还是那么两点。

二、牛顿公式推导

2.1 一维函数求最小值

设函数$f(x)$，$x\in R$，将函数进行二阶泰勒展开：
$$f(x_k)=f(x_{k-1})+f^{\prime }(x_{k-1})(x_k-x_{k-1})+\frac{1}{2}{f}^{\prime \prime }(x_{k-1})(x_k-x_{k-1}{)}^2+o$$
对其求导，令导数为0：
$$x_k=x_{k-1}-\frac{f^{\prime }(x_{k-1})}{f^{\prime \prime }(x_{k-1})}$$

2.2 二（多）维函数求最小值

直接上多维确实是很抽象，我们先从二维出发，看一下具体是怎么来的。设二元函数$f(x,y)$在点$(a,b)$进行二阶泰勒展开：
$$\begin{array}{rl}f(x,y)\approx & f(a,b)+\left[\frac{\partial f}{x}(a,b),\frac{\partial f}{y}(a,b)\right]\left[\begin{array}{c}x-a\\ y-b\end{array}\right]\\ +& \frac{1}{2}\left[\begin{array}{cc}x-a& y-b\end{array}\right]\left[\begin{array}{cc}\frac{{\partial }^{2}f}{\partial x^2}(a,b)& \frac{{\partial }^{2}f}{\partial x\partial y}(a,b)\\ \frac{{\partial }^{2}f}{\partial y\partial x}(a,b)& \frac{{\partial }^{2}f}{\partial y^2}(a,b)\end{array}\right]\left[\begin{array}{c}x-a\\ y-b\end{array}\right]\\ =& f(a,b)+(x-a)f_x^{\prime }(a,b)+(y-b)f_y^{\prime }(a,b)\\ +& \frac{1}{2!}(x-a{)}^2{f}_{xx}^{\prime \prime }(a,b)+\frac{1}{2!}(x-a)(y-b)f_{xy}^{\prime \prime }(a,b)\\ +& \frac{1}{2!}(x-a)(y-b)f_{yx}^{\prime \prime }(a,b)+\frac{1}{2!}(y-b{)}^2{f}_{yy}^{\prime \prime }(a,b)\end{array}$$
从结果可以发现一阶导数是两个向量的偏导矩阵，二阶导数是两个向量的二阶偏导矩阵。

二、高斯–牛顿公式推导

2.1 基础知识

设有向量函数f(x)及向量x：
$$f:[f_1(\mathbf{x}),f_2(\mathbf{x}),\dots ,f_m(\mathbf{x})]$$
$$x:[x_1,x_2,\dots ,x_n]$$
接下来回顾雅可比矩阵，它就是f对x的一阶导数，只不过上升到矩阵层面了：

$$J_f=\left[\begin{array}{c}\frac{\partial f}{\partial x_1},\frac{\partial f}{\partial x_2},\dots ,\frac{\partial f}{\partial x_n}\end{array}\right]=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x_1}& \cdots & \frac{\partial f_1}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_m}{\partial x_1}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]$$
对向量f进行泰勒展开（按照泰勒公式），进行一阶近似：

$$f(\mathbf{x}+\mathbf{\Delta }x)=f(\mathbf{x})+\nabla f(\mathbf{x})\Delta x+\frac{1}{2}\Delta x^{T}H(f(\mathbf{x}))\Delta x$$
对$f(x_0)$求导并令其导数为0：
$$x_{n+1}=x_n-[H(f(x_n)){]}^{-1}\nabla f(x_n)$$

何为非线性，最直观的理解就是拟合的图像不是一条直线，可能是各种各样的曲线（各种函数的组合），所以在此处不能像上面那个例子一样求个导就能求出x的值，就像$e^x$去求导，永远不可能把x消掉，求出x的具体值，我们只能去一点一点逼近x，直到偏差在可接受的范围内。

2.2 抛出问题

现有一个向量函数$f$，有m个观测点（上图中的散点），每个点取决于p个变量（$x$矩阵有p个分量）。模型函数如下，$\beta$参数来反映函数：
$$f(x_1,x_2,\dots ,x_p;{\beta }_1,{\beta }_2,\dots ,{\beta }_n)$$

2.3 优化目标函数

根据上面的例子，我们知道优化函数是向量函数预测值和真实值只差的平方和，表示如下：
$$minS=\sum _{i=1}^m(f(x_i;\beta )-y_i{)}^2$$
第i次观测点的预测偏差：
$$r_i=f(x_i;\beta )-y_i$$
将ri表示为矩阵:
$$r=[r_1,\dots ,r_m{]}^T$$
于是目标函数$S$可写为：
$$S=\sum _{i=1}^m{r}_i^2=r^{T}r\text{\hspace{1em}(1x1)}$$
接下来要做的就是让目标函数最小化，换而言之准备泰勒展开求导吧，$S$对$\beta$求导，先对$r$求导再对$\beta$求导。
$$\frac{\partial S}{\partial {\beta }_j}=2\sum _{i=1}^m{r}_i\frac{\partial r_i}{\partial {\beta }_j}$$
梯度为：
$$\nabla S(\beta )=\left[\begin{array}{c}\frac{\partial S}{\partial {\beta }_1},\dots ,\frac{\partial S}{\partial {\beta }_n}\end{array}\right]$$
用$J(r(\beta ))$代换，梯度为：
$$\nabla S=2J^{T}r$$
接下来是高斯–牛顿法的重点，用$J$去近似$H$，实际上就是去求二阶导，再去近似：
$$H=\frac{{\partial }^{2}S}{\partial {\beta }_k{\beta }_j}=2\sum _{i=1}^m(\frac{\partial r_i}{\partial {\beta }_k}\frac{\partial r_i}{\partial {\beta }_j}+r_i\frac{{\partial }^2{r}_i}{\partial {\beta }_k\partial {\beta }_j})$$
舍弃二阶导数项：
$$H\approx 2\sum _{i=1}^m\frac{\partial r_i}{\partial {\beta }_k}\frac{\partial r_i}{\partial {\beta }_j}=2\sum _{i=1}^m{J}_{ik}{J}_{ij}$$
即：
$$H=2J^{T}J$$
代入牛顿法结论：
$$x_{n+1}=x_n-[H(f(x_n)){]}^{-1}\nabla f(x_n)$$
$${\beta }_{(k+1)}={\beta }_{(k)}-H^{-1}g={\beta }_{(k)}-(2J^{T}J{)}^{-1}2J^{T}r$$
整理完成：
$${\beta }_{(k+1)}={\beta }_{(k)}-(J^{T}J{)}^{-1}{J}^T{r}_{\beta }$$

总结