视觉惯性单目SLAM （三）优化方法-CSDN博客

本文链接：https://blog.csdn.net/MyArrow/article/details/56279273

1. 最小二乘法

最小二乘法：Least Squares Method （LSM）
函数： $y=f(x,\theta)$
- $\theta$ 为待估参数(可为向量或标量)
- $x,y$ 常用术语

x	y
自变量（independent variable）	因变量（dependent variable）
解释变量（explanatory variable）	被解释变量（explained varible）
原因变量（causal variable）	结果变量（effect variable）

目标函数：
$Q = \sum i = 0 n (y i - f (x i, θ)) 2$ $Q = \sum_{i=0}^n \left({y_i - f(x_i, \theta) }\right)^2$
目标：求使残差的平方和最小的待估参数
最小二乘法分类：
- 线性（linear or ordinary least squares(OLS)）： $函数f是参数\theta的线性函数，即\theta都是一次的，对其求导为常数$
- 非线性（non-linear least squares）： $函数f是参数\theta的非线性函数，即\theta不都是一次的，对其求导不都为常数$
线性最小二乘的解：是封闭形式(closed-form)的，即 $对于Ax=b，则有x=(A^T A)^{-1}A^Tb$ (A不一定为方阵)
非线性最小二乘的解：不是封闭形式(closed-form)，通常用迭代法求解
超定方程组（overdetermined systems）：方程组中：方程的个数 $>$ 未知量的个数
残差（residuals）： $观测值(实际值) - 模型的预测值$
估计量（estimator)：是指计算系数(待估参数)的方程
估计值（estimate）：是指估计出来的系数(待估参数)的值
MSE(Mean Squared Error)：平均预测误差平方和 (评价标准)

$M S E = 1 m \sum i = 0 m (y s i - y α i) 2$ $MSE=\frac {1}{m} \sum_{i=0}^m (y_i^s - y_i^{\alpha})^2$
$\quad y_i^s：y_i的预测值，y_i^{\alpha}的实际值，m：样本数$
MAE(Mean Absolute Error):平均预测误差绝对值 (评价标准)

$M A E = 1 m \sum i = 0 m | y s i - y α i |$ $MAE=\frac {1}{m} \sum_{i=0}^m |y_i^s - y_i^{\alpha}|$

2. 优化方法分类

$\color {red}{优化方法分类}$ ：
- 对于非线性优化问题，不能象线性最小二乘法那样用求多元函数极值的办法来得到参数估计值，而需要采用复杂的优化算法来求解。常用的算法有两类：
- 搜索算法
- 迭代算法
$\color {red}{搜索算法的思路}$ ：
- 按一定的规则选择若干组参数值，分别计算它们的目标函数值并比较大小；选出使目标函数值最小的参数值，同时舍弃其他的参数值；然后按规则补充新的参数值，再与原来留下的参数值进行比较，选出使目标函数达到最小的参数值。如此继续进行，直到选不出更好的参数值为止。以不同的规则选择参数值，即可构成不同的搜索算法。常用的方法有单纯形搜索法、复合形搜索法、随机搜索法等。
$\color {red}{迭代算法的思路}$ ：
- 是从参数的某一初始猜测值 $θ(0)$ 出发，然后产生一系列的参数点 $θ(1)、θ(2)…$ ，如果这个参数序列收敛到使目标函数极小的参数点，那么对充分大的 $N$ 就可用 $θ(N)$ 作为结果。迭代算法的一般步骤是：
  　　①　给出初始猜测值θ(0)，并置迭代步数 $i＝1$ 。
  　　②　确定一个向量 $v(i)$ 作为第 $i$ 步的迭代方向。
  　　③　用寻优的方法决定一个标量步长 $ρ(i)$ ，使得 $Q(θ(i))＜Q(θ(i-1))$ ，其中 $θ(i)＝θ(i-1)+ρ(i)v(i)$ 。
  　　④　检查停机规则是否满足，如果不满足,则将i加1再从②开始重复；如果满足，则取θ(i)为结果。
非线性最小二乘法除可直接用于估计静态非线性模型的参数外，在时间序列建模、连续动态模型的参数估计中，也往往遇到求解非线性最小二乘问题。
二阶以上多项式的曲线拟合属于非线性最小二乘法。
$\color {red}{迭代法}$ ：在每一步update未知量逐渐逼近解，可以用于各种各样的问题（包括最小二乘），比如求的不是误差的最小平方和而是最小立方和
- $\color {blue}{梯度下降是迭代法}$ ：可以用于求解最小二乘问题（线性和非线性都可以）
- $\color {blue}{高斯-牛顿迭代法}$ ：一种经常用于求解非线性最小二乘的迭代法（一定程度上可视为标准非线性最小二乘求解方法）
- $\color {blue}{Levenberg-Marquardt (L-M)迭代法}$ ：用于求解非线性最小二乘问题，就结合了梯度下降和高斯-牛顿法。

3. 线性最小二乘法

3.1 简单问题求解步骤

1）列出目标函数
2）对每个待估参数求偏导，并令其等于0
3）把第2)步的所有方程组成一个方程组，解此方程组即可得待估参数的值

3.2 通用问题求解方案

对于超定方程（overdetermined system）
$y i = \sum j = 1 n x i, j β j (i = 1, 2, \dots, m)$ $y_i = \sum_{j=1}^n x_{i,j} \beta_j (i=1, 2, \cdots, m)$
1） $m$ ：线性方程组个数
2） $n$ ：未知系数个数（ $\beta_1, \beta_2, \cdots, \beta_n$ )
3） $m>n，x_{i,1} =1$
4）矩阵形式： $Y = X β$ $Y=X \beta$
$X = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ x 1, 1 x 2, 1 ⋮ x m, 1 x 1, 2 x 2, 2 ⋮ x m, 2 \dots \dots ⋱ \dots x 1, n x 2, n ⋮ x m, n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥, β = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ β 1 β 2 ⋮ β n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥, y = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ y 1 y 2 ⋮ y n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥$ $X=\begin{bmatrix} x_{1,1} & x_{1,2} & \cdots &x_{1,n} \\ x_{2,1} & x_{2,2} & \cdots &x_{2,n} \\ \vdots & \vdots & \ddots & \vdots \\ x_{m,1} & x_{m,2} & \cdots &x_{m,n} \\ \end{bmatrix}, \quad \beta =\begin{bmatrix} \beta_1 \\ \beta_2 \\ \vdots \\ \beta_n \end{bmatrix},\quad y =\begin{bmatrix} y_1 \\ y_2 \\ \vdots \\ y_n \end{bmatrix}$
这样的超定方程组经常没有解，所以不得不找到参数 $\beta$ ，使它更好地拟合方程组
定义目标函数：
$S (β) = \sum i = 1 m (y i - \sum j = 1 n x i, j β j) 2 = | | y - X β | | 2$ $S(\beta) =\sum_{i=1}^m \left(y_i - \sum_{j=1}^n x_{i,j} \beta_j \right)^2 = ||y-X\beta ||^2$
优化目标：
$β^= a r g m i n β S (β)$ $\hat \beta = \mathop{arg \; min}_\beta S(\beta)$
解的常规方程变为：
$X β^= y \Rightarrow (X T X) β^= X T y \Rightarrow β^= (X T X) - 1 X T y$ $X\hat \beta = y \Rightarrow (X^TX)\hat \beta = X^Ty \Rightarrow \hat \beta = (X^TX)^{-1}X^Ty$
XTX ：格拉姆矩阵（Gramian Matrix），是一个方阵，其性质为：
- 是半正定的（positive semi-definite matrix）
  $G = A T A = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ a T 1 a T 2 ⋮ a T n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ [a 1 a 2 \dots a T n] = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ a T 1 a 1 a T 2 a 1 ⋮ a T n a 1 a T 1 a 2 a T 2 a 2 ⋮ a T n a 2 \dots \dots ⋱ \dots a T 1 a n a T 2 a n ⋮ a T n a n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥$ $G = A^TA= \begin{bmatrix} a_1^T \\ a_2^T \\ \vdots \\ a_n^T \end{bmatrix} \begin{bmatrix} a_1 & a_2 & \cdots &a_n^T \end{bmatrix}=\begin{bmatrix} a_1^Ta_1 & a_1^Ta_2 & \cdots & a_1^Ta_n \\ a_2^Ta_1 & a_2^Ta_2 & \cdots & a_2^Ta_n \\ \vdots & \vdots & \ddots & \vdots \\ a_n^Ta_1 & a_n^Ta_2 & \cdots & a_n^Ta_n \end{bmatrix}$

4. 非线性最小二乘法

$\color {red}{基本思想}$ ：用线性函数来近似非线性函数，再模仿线性最小二乘法求解
$\color {red}{非线性最小二乘法通用描述}$

x∗=argminx(12∑i=1m(fi(x))2)=argminx(12||f(x)||2)=argminx(12f(x)Tf(x))
- $f(x)$ ：为向量函数（即此向量中至少有一个元素是自变量 $x$ 的函数），它是一个向量
- $f_i(x)：R^n \to R, \quad i=1, 2, \cdots, m$ ，它为给定的 $m个残差函数(residual function)$
- $x=\begin{bmatrix} x_1 & x_2 & \cdots & x_n \end{bmatrix}^T$
- $m \ge n$
$\color {red}{代价函数F可微(其值为标量)}，其泰勒展开式为：$

F(x+h)=F(x)+hTg+12hTHh+O(||h||3)
- $\color {red}{g为梯度(或叫做只有一行的Jacobian \; Matrix)，即一阶偏导：}$
  
  $g = F' (x) = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ \partial F \partial x 1 (x) \partial F \partial x 2 (x) ⋮ \partial F \partial x n (x) ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥$ $g=F'(x) =\begin{bmatrix} \frac {\partial F}{\partial x_1}(x) \\ \frac {\partial F}{\partial x_2}(x) \\ \vdots \\ \frac {\partial F}{\partial x_n}(x) \\ \end{bmatrix}$
- $\color {red}{h向量的每个元素为无穷小，即x+h在x的邻域内}$
- $\color {red}{H为海森矩阵（Hessian \; Matrix），即二阶偏导}$
  $H = F'' (x) = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ \partial 2 F \partial x 1 x 1 \partial 2 F \partial x 2 x 1 ⋮ \partial 2 F \partial x n x 1 \partial 2 F \partial x 1 x 2 \partial 2 F \partial x 2 x 2 ⋮ \partial 2 F \partial x n x 2 \dots \dots ⋱ \dots \partial 2 F \partial x 1 x n \partial 2 F \partial x 2 x n ⋮ \partial 2 F \partial x n x n ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥$ $H=F''(x) =\begin{bmatrix} \frac {\partial ^2F}{\partial x_1x_1} & \frac {\partial ^2F}{\partial x_1x_2} & \cdots & \frac {\partial ^2F}{\partial x_1x_n} \\ \frac {\partial ^2F}{\partial x_2x_1} & \frac {\partial ^2F}{\partial x_2x_2} & \cdots & \frac {\partial ^2F}{\partial x_2x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac {\partial ^2F}{\partial x_nx_1} & \frac {\partial ^2F}{\partial x_nx_2} & \cdots & \frac {\partial ^2F}{\partial x_nx_n} \\ \end{bmatrix}$
  注：具体推导见视觉惯性单目SLAM （四）-泰勒展开式

4.1 梯度下降法（Gradient Descent）

$\color {red}{一阶优化算法}$
$\color {red}{用于计算函数的局部最小值}$
$\color {red}{迭代增量：在当前点的梯度的负值，再乘以一个比例值 \lambda}$
$\color {red}{迭代步骤：}$
1）初始化： $k=0, x=x_0$
2）当 $k<k_{max}$
$\quad \quad x_{k+1} = x_k - \lambda F'(x_k)= x_k - \lambda g(x_k) = x_k - \lambda \nabla F(x_k)$

4.2 牛顿法（Newton Method )

$\color {red}{二阶近似优化算法}$
$\color {red}{代价函数的二阶泰勒展开}$ ：
$F (x + h) \approx F (x) + F' (x) h + 1 2 F'' (x) h 2$ $F(x+h) \approx F(x) + F'(x)h + \frac {1}{2}F''(x)h^2$
$\color {red}{x为一维时，优化方向h为 (对h求一阶导，并令其为0，求解h即可)：}$

$h = - F ' ( x ) F '' ( x )$ $h=-\frac {F'(x)}{F''(x)}$
$\color {red}{x为高维时，优化方向h为： }$

$F (x + h) \approx F (x) + h T \nabla F (x) + 1 2 h T H (x) h$ $F(x+h) \approx F(x) + h^T \nabla F(x) + \frac {1}{2}h^TH(x)h$
$h \approx - H (x) - 1 \nabla F (x) = - H (x) - 1 g$ $h \approx -H(x)^{-1} \nabla F(x) = -H(x)^{-1}g$
$\color {red}{迭代步骤：}$
1）初始化： $k=0, x=x_0$
2）当 $k<k_{max}$
$\quad \quad x_{k+1} = x_k - \lambda H(x_k)^{-1} \nabla F(x_k)= x_k - \lambda H(x_k)^{-1} g(x_k)$
发现海森矩阵的逆是非常昂贵的，近似的方法被使用，近似的方法有：
- 共轭梯度下降法（conjugate gradient method）
- 拟牛顿法（quasi-newton method）
$\color {red}{牛顿法与梯度下降法的比较，红色的为牛顿法}$

4.3 高斯-牛顿法（Gauss-Newton Methods）

$\color {red}{经常用于求解非线性最小二乘问题}$
原理:
- 高斯—牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。

4.3.1 方法描述

$\color {red}{方法描述:}$
- 给定 $m$ 个函数 $r=(r_1, \cdots, r_m)$ (经常叫做残差， $r$ 为向量值函数），每个函数 $r_i(\beta)$ 有 $n$ 个变量 $\beta = (\beta_1, \cdots, \beta_n)$ ，且 $m \ge n$ ，
- $\color {blue}{高斯-牛顿迭代法的目标}$ ：找到 $\beta$ 的最优解，使得残差的平方和最小：：
  
  $S (β) = \sum i = 1 m r 2 i (β) = r (β) T r (β) = | | r (β) | | 2$ $S(\beta) = \sum_{i=1}^m r_i^2(\beta) = r(\beta)^Tr(\beta)=||r(\beta)||^2$
- 以初始值 $\beta^{(0)}$ 开始迭代，则迭代方法如下：
  
  $β (s + 1) = β (s) - (J T r J r) - 1 J T r r (β (s))$ $\beta^{(s+1)} = \beta^{(s)} - (J_r^TJ_r)^{-1}J_r^Tr(\beta^{(s)})$
- 若 $r和\beta$ 都为列向量，则 $\color {red}{雅可比矩阵（Jacobian \; matrix）}$ 的项为：
  $(J r) i, j = \partial r i ( β ( s ) ) \partial β j$ $(J_r)_{i,j} = \frac {\partial r_i(\beta^{(s)})}{\partial \beta_j}$
  $注： J r 是向量函数 r 对 β 向量的雅可比矩阵$ $注： J_r是向量函数r对\beta向量的雅可比矩阵$
- 如果 $m==n$ ，则迭代简化为：
  $β (s + 1) = β (s) - (J r) - 1 r (β (s))$ $\beta^{(s+1)} = \beta^{(s)} - (J_r)^{-1}r(\beta^{(s)})$
- 在数据拟合中，需要回归的函数为： $y=f(x,\beta)$ ，此函数拟合数据点 $(x_i,y_i)$ ，则 $r_i$ 为：
  $r i (β) = y i - f (x i, β)$ $r_i(\beta) = y_i - f(x_i, \beta)$
  $则高斯-牛顿迭代法可用函数f的Jacobian Matrix表示：$
  $β (s + 1) = β (s) + (J T f J f) - 1 J T f r (β (s))$ $\beta^{(s+1)} = \beta^{(s)} + (J_f^TJ_f)^{-1}J_f^Tr(\beta^{(s)})$
  $(J f) i, j = \partial f ( x i , β ( s ) ) \partial β j$ $(J_f)_{i,j} = \frac {\partial f(x_i, \beta^{(s)})}{\partial \beta_j}$
  $注： J f 是向量函数 f 对 β 向量的雅可比矩阵$ $注： J_f是向量函数f对\beta向量的雅可比矩阵$

4.3.2 方法推导

$\color {red}{从牛顿法出发进行推导}$
$\color {red}{根据牛顿法使用S(\beta)或S最小的\beta}，其递归关系为$ ：
$S = \sum i = 1 m r 2 i (S 是多元标量函数，其值为标量)$ $S=\sum_{i=1}^m r_i^2 \quad (S是多元标量函数，其值为标量)$
$β s + 1 = β s - H - 1 g, (g 为 S 的梯度， H 为 S 的海森矩阵)$ $\beta^{s+1} = \beta^s - H^{-1}g, \quad (g为S的梯度，H为S的海森矩阵)$
$\color {red}{则梯度为：}$
$g j = 2 \sum i = 1 m r i \partial r i \partial β j$ $g_j = 2\sum_{i=1}^mr_i \frac{\partial r_i}{\partial \beta_j}$
海森矩阵的元素由梯度元素对每个自变量 $\beta_k$ 求偏导组成：
$H j k = \partial g j \partial β k = 2 \sum i = 1 m (\partial r i \partial β k \partial r i \partial β j + r i \partial 2 r i \partial β j \partial β k)$ $H_{jk} = \frac {\partial g_j}{\partial \beta_k} = 2\sum_{i=1}^m \left(\frac {\partial r_i}{\partial \beta_k}\frac{\partial r_i}{\partial \beta_j} + r_i \frac{\partial^2 r_i}{\partial \beta_j \partial \beta_k}\right)$
高斯-牛顿法忽略了二阶偏导数，即上式中的第二项，因为其值较小，则Hessian矩阵近似表示为：
$H j k \approx 2 \sum i = 1 m (\partial r i \partial β k \partial r i \partial β j) = 2 \sum i = 1 m (J i k J i j) = 2 \sum i = 1 m (J i j J i k)$ $H_{jk} \approx 2\sum_{i=1}^m \left(\frac {\partial r_i}{\partial \beta_k}\frac{\partial r_i}{\partial \beta_j} \right) = 2\sum_{i=1}^m(J_{ik}J_{ij}) = 2\sum_{i=1}^m(J_{ij}J_{ik})$
$J_{ij}：是雅可比矩阵J_r的元素（i行，j列）$
梯度和海森矩阵的矩阵表示为：（即使用雅可比矩阵来表示梯度和海森矩阵）
$g = 2 J T r r = 2 J T r (β s) r (β s)$ $g = 2J_r^Tr = 2J_r^T(\beta^s)r(\beta^s)$
$H \approx 2 J T r J r = 2 J r (β s) T J r (β s)$ $H \approx 2J_r^TJ_r = 2J_r(\beta^s)^TJ_r(\beta^s)$
则高斯-牛顿法的迭代公式为：
$β s + 1 = β s - H - 1 g = β s - (J T r J r) - 1 J T r r (β s)$ $\beta^{s+1} = \beta^s - H^{-1}g = \beta^s - (J_r^TJ_r)^{-1}J_r^Tr(\beta^s)$

4.4 列文伯格-马夸尔特法(Levenberg–Marquardt algorithm)

Levenberg–Marquardt algorithm：简记为：LMA或LM
LM是一个著名的阻尼最小二乘法（Damped Least-Squares < DLS>），用于求解非线性最小二乘问题，
LMA特别擅长最小二乘曲线拟合问题
LMA与GNA(Gauss−NewtonAlgorithm)和GD(GradientDescent)的比较
- LMA介于GNA与GD之间
- LMA比GNA比加健壮：在许多情况，即使从远离最后的极小值点开始，它也能找到解
- 对于性质优良的函数（如凸函数）和合理的开始参数，LMA比GNA慢一些
- LMA也可以看作是使用信赖域方法的高斯–牛顿法
- LM法就是在高斯-牛顿法的基础上加入了一个变量因子
- LMA的迭代公式为：
  $β s + 1 = β s - H - 1 g = β s - (J T r J r + λ I) - 1 J T r r (β s) = β s - [J T r J r + λ d i a g (J T r J r)] - 1 J T r r (β s)$ $\beta^{s+1} = \beta^s - H^{-1}g = \beta^s - (J_r^TJ_r + \lambda I)^{-1}J_r^Tr(\beta^s) = \beta^s - [J_r^TJ_r + \lambda diag(J_r^TJ_r)]^{-1}J_r^Tr(\beta^s)$

4.4 总结

非线性最小二乘法求解方法总结
映射 $f$ ： $R^n \to R$

算法	近似	迭代公式
梯度下降法	一阶近似	$\quad \quad x_{k+1} = x_k - \lambda F'(x_k)= x_k - \lambda g(x_k) = x_k - \lambda \nabla F(x_k)$
牛顿法	二阶近似	$\quad \quad x_{k+1} = x_k - \lambda H(x_k)^{-1} \nabla F(x_k)= x_k - \lambda H(x_k)^{-1} g(x_k)$
高斯-牛顿法	近似二阶	$\beta^{(s+1)} = \beta^{(s)} - (J_r^TJ_r)^{-1}J_r^T\, r(\beta^{(s)})， J_r是向量函数r对\beta向量的雅可比矩阵$ $(J_r)_{i,j} = \frac {\partial r_i(\beta^{(s)})}{\partial \beta_j}$
列文伯格-马夸尔特法	近似二阶	$\beta^{(s+1)} = \beta^{(s)} - [J_r^TJ_r + \lambda I]^{-1}J_r^T\, r(\beta^{(s)})$ 或 $\beta^{(s+1)} = \beta^{(s)} - [J_r^TJ_r + \lambda diag(J_r^TJ_r)]^{-1}J_r^T\,r(\beta^{(s）}),J_r是向量函数r对β向量的雅可比矩阵$ $(J_r)_{i,j} = \frac {\partial r_i(\beta^{(s)})}{\partial \beta_j}$

5. 线性与非线性最小二乘法的比较

比较属性	线性最小二乘法	非线性最小二乘法
求解方法	求多元函数极值的方法得待估参数 1）列出目标函数 2）对参数求导并令其为0 3）求解方程组	1）搜索算法 2）迭代法： $\quad$ 高斯-牛顿法
convex	convex	convex 或 non-convex