非线性优化问题基本形式概述

非线性优化问题以及在视觉SLAM中的应用

1.0 最小二乘基础概念

  • 定义

(quad) 找到一个 n 维的变量 (mathbf{x}^{*} in mathbb{R}^{n}) , 使得损失函数 (F(mathbf{x})) 取局部最小值：

[F(mathbf{x})=frac{1}{2} sum_{i=1}^{m}left(f_{i}(mathbf{x})right)^{2}
]

(quad)其中 (f_{i}) 是残差函数, 比如测量值和预测值之间的差, 且有 (m geq n) 。 部最小值指对任意 (left|mathbf{x}-mathbf{x}^{*}right| 有 (Fleft(mathbf{x}^{*}right) leq F(mathbf{x}))
(quad)损失函数泰勒展开，假设损失函数 (F(mathbf{x})) 是可导并且平滑的, 因此, 二阶泰勒展开:

[F(mathbf{x}+Delta mathbf{x})=F(mathbf{x})+mathbf{J} Delta mathbf{x}+frac{1}{2} Delta mathbf{x}^{top} mathbf{H} Delta mathbf{x}+Oleft(|Delta mathbf{x}|^{3}right)
]

这里要着重注意一下，这里的 (mathbf{J}) 和 (mathbf{H}) 都是每一个残差项的雅可比堆叠(计算)而来，实际上对于初学者来说并不直观，后面我们会以一个曲线拟合和 (BA) 问题来详细分析一下如何通过连加来推算到 (mathbf{J}) 和 (mathbf{H})

(quad)其中 (mathbf{J}) 和 (mathbf{H}) 分别为损失函数 (F) 对变量 (x) 的一阶导和二阶导矩阵，也就是我们通常所说的雅可比矩阵和海塞矩阵。(这里的 (mathbf{x}) 包含了所有待优化的变量，在视觉SLAM问题中，一般是相机的 Pose 和已经三角化的点的坐标或者逆深度，且由于相机一般能观测到的3D点的个数是有限的，因此其雅可比矩阵也就是稀疏的，只有两个地方的雅可比求导是不为0的，参考14讲P247，那么 (J_{i,j}^TJ_{i,j})，则只有四个地方是不为0的)。

  • 损失函数泰勒展开的性质

(quad) 忽略泰勒展开的高阶项,损失函数变成了二次函数,可以轻易得到如下性质：

  1. 如果在点 (x_s) 处有导数为 (0) ,则称这个点为稳定点。
  2. 在点 (x_s) 处对应的 Hessian 为 (mathbf{H})：
    • 如果是正定矩阵，即它的特征值都大于 (0)，则在 (x_s) 处有 (F (x)) 为局部最小值。
    • 如果是复定矩阵，即它的特征值都小于 (0)，则在 (x_s) 处有 (F (x)) 为局部最大值。
    • 如果是不定矩阵，即它的特征值大于 (0) 也有小于 (0) 的,则 (x_s) 处为鞍点。
  • 求解方法主要有以下两种：
    • 直接求解：线性最小二乘（这里不再赘述，为线性代数的内容，超定方程的通解为 (x=(A^TA)^{-1}A b)）
    • 迭代下降法：适用于线性和非线性最小二乘

2.0 迭代下降求解方式

  • 迭代法初衷：

    找到一个下降方向使得损失函数随着 (x) 的迭代逐渐减少直到 (x^*)。

    [F(x_{k+1})

    分两个步骤；第一，找到下降方向单位向量 (d) ，第二，确定下降的步长 (a)。

    假设 (a) 足够的小，又因为 (d) 为单位向量，因此可以将 (ad) 看作是一个微小的变化量 (triangle{x})，我们可以对损失函数进行一阶泰勒展开：

    [F(mathbf{x}+a mathbf{d}) approx F(mathbf{x}) + amathbf{J}mathbf{d}
    ]

    只需要寻找下降方向，满足：

    [mathbf{Jd}

    通过 line search 的方法找到下降的步长：(a^*=argmin_{a>0} [F(x+ad)])

    2.1 最速下降法: 适用于迭代的开始阶段

    适用于迭代的开始阶段

    (quad) 从下降方向的条件(单位向量)可以知道: (mathbf{Jd=||J||}costheta) ，其中 (theta) 表示的是下降方向和梯度方向的夹角. 当 (theta = pi) 有:

    这里为什么能写成向量的内积运算,笔者在刚开始看起来还认为是两个矩阵相乘法,却直接写成了内积运算，仔细思索发现 (d) 其实上是一个和 (x) 相同维度的单位向量，其纬度为 (ntimes 1) ，而雅可比矩阵

    [mathbf{d=frac{-J^T}{||J||}}
    ]

    (quad)即梯度的负方向为最速下降方向。缺点:最优值附近震荡，收敛慢。

    2.2 牛顿法: 适用于最优值附近

    在局部最优点 (x^∗) 附近,如果 (x + ∆x) 是最优解,则损失函数对 (∆x) 的导数等于 (0),对公式 (2) 取一阶导有:

    [begin{align*}
    frac{partial}{partial Delta x}left (F(mathbf{x})+mathbf{J} Delta mathbf{x}+frac{1}{2} Delta mathbf{x}^{top} mathbf{H} Delta mathbf{x} right) &=mathbf{J^T} + mathbf{H}Delta x =0
    end{align*}
    ]

    得到:(∆x = -mathbf{H^{-1}J^T}) 。缺点:二阶导矩阵计算复杂。

    这里我们其实既可以看作是多个残差的分量相加后组成的 (H)，也可以看作是每个残差单独的 (H)。

    2.3 阻尼法：防止 (Delta x) 的模过大

    将损失函数的二阶泰勒展开记作：

    [F(mathbf{x}+Delta x)approx L(Delta x) = F(mathbf{x})+mathbf{J} Delta mathbf{x}+frac{1}{2} Delta mathbf{x}^{top} mathbf{H} Delta mathbf{x}
    ]

    求以下函数的最小化:

    [Delta x = arg underset{Delta x}{min} left { L left (Delta xright ) + frac{1}{2}mu Delta x^TDelta x right }
    ]

    其中,(μ ≥ 0) 为阻尼因子, $ frac{1}{2}mu Delta x^TDelta x $是惩罚项。对新的损失函数求一阶导,并令其等于 (0) 有:

    [mathbf{L^`(Delta x)} + mu mathbf{Delta x} = 0 \
    (mathbf{H}+mumathbf{I})Delta x = -mathbf{J^T}
    ]

    2.4 Gauss-Newton 和 LM

    残差函数 (f(x)) 为非线性函数，对其进行一阶泰勒近似有：

    [f(x+Delta x)approx ell (Delta x) equiv f(x)+JDelta x
    ]

    带入损失函数：

    [begin{aligned}
    F(mathbf{x}+Delta mathbf{x}) approx L(Delta mathbf{x}) & equiv frac{1}{2} ell(Delta mathbf{x})^{top} ell(Delta mathbf{x}) \
    & =frac{1}{2} mathbf{f}^{top} mathbf{f}+Delta mathbf{x}^{top} mathbf{J}^{top} mathbf{f}+frac{1}{2} Delta mathbf{x}^{top} mathbf{J}^{top} mathbf{J} Delta mathbf{x} \
    & =F(mathbf{x})+Delta mathbf{x}^{top} mathbf{J}^{top} mathbf{f}+frac{1}{2} Delta mathbf{x}^{top} mathbf{J}^{top} mathbf{J} Delta mathbf{x}
    end{aligned}
    ]

    这里我们假设暂时只关注其中的一项（其实也可以是所有损失项的叠加，最终形式是一样的）。在 (x) 处进行的泰勒展开，则认为 (x) 是已知的，现在的损失函数是一个关于 (Delta x) 的函数，其最小值，则令关于 (Delta x) 的导数为 (0) 即可。可以得到：

    [mathbf{(J^T J)}Delta x_{gn}=-mathbf{J^T f}
    ]

    上面这个形式就是我们在论文或者各种SLAM问题中经常见到的形式了，(mathbf{H Delta x =b})，也叫做 normal equation

    曲线拟合理解

    现在我们通过非线性最小二乘来进行线性拟合实验，将理论应用于实际中去。假设曲线方程为：

    [y=exp (ax^2+bx+c)
    ]

    其中设 (a=1,b=2,c=3) 。

    现在加入噪声项生成带有高斯分布的噪声数据，当然不是高斯分布的数据也是可以的，但是在自己实验的时候尽量不要出现外点数据，因为我们并没有处理外点数据的策略。则生成数据的方程为：

    [y=exp (ax^2+bx+c) + w
    ]

    其中 (w) 为符合高斯分布的噪声数据。

    通过如下程序生成观测数据：

    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
int N = 100;                                                    // 数据点
double w_sigma = 1.0;                            // 噪声Sigma值
vector x_data, y_data;        // 数据
  for (int i = 0; i

    接下来我们关心雅可比如何计算，误差项 (f_i(a,b,c)) 可以写成如下形式：

    [f_i(a,b,c)=y_i-exp(a_ex_i^2+b_ex_i+c_e)
    ]

    我们知道这两项相减是绝对不可能相等的，因为在生成数据的时候加入了高斯噪声。我们这里有 (N) 个观测，即 (iin (1-100))，我们将其写成连加的形式

    [F(X)=sum_{i=1}^{N}left (y_i-exp(a_ex_i^2+b_ex_i+c_e) right)^2
    ]

    该式中右边就是残差项的具体形式，我们对其进行平方，防止负的残差和正的残差抵消的情况，前面我们已经说过可以将残差项通过一阶泰勒展开进行近似，然后进行平方得到每一个残差项的具体形式：

    (f(x+Delta x)approx f(x)+J(x)Delta x)

    [begin{aligned}
    frac{1}{2}|f(boldsymbol{x})+boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}|^{2} & =frac{1}{2}(f(boldsymbol{x})+boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x})^{T}(f(boldsymbol{x})+boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}) \
    & =frac{1}{2}left(|f(boldsymbol{x})|_{2}^{2}+2 f(boldsymbol{x})^{T} boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}+Delta boldsymbol{x}^{T} boldsymbol{J}(boldsymbol{x})^{T} boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}right) \
    &=Omega(Delta x)
    end{aligned}
    ]

    此时由于某时刻的观测已知，因此误差项是一个关于 (Delta x) 的二次函数，求该项的最小值只要让关于 (Delta x) 的导数为 (0) 即可。求导后可得：

    [begin{array}{l}
    2 boldsymbol{J}(boldsymbol{x})^{T} f(boldsymbol{x})+2 boldsymbol{J}(boldsymbol{x})^{T} boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}=mathbf{0} \
    boldsymbol{J}(boldsymbol{x})^{T} boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}=-boldsymbol{J}(boldsymbol{x})^{T} f(boldsymbol{x})
    end{array}
    ]

    这里我们简单的记：

    [boldsymbol{J}(boldsymbol{x})^{T} f(boldsymbol{x}) = mathbf{eta}\

    boldsymbol{J}(boldsymbol{x})^{T} boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}=mathbf{HDelta x}
    ]

    即我们常见的形式：

    读者要注意到这里的 (b) 其实就是上面的 (-eta)

    [mathbf{HDelta x=b}
    ]

    这里我们假设残差项记为 (mathbf{e_i}) 一共有 (N) 个观测，则有 (N) 个残差项。

    [F(X)=mathbf{e_1^2 + e_2^2+ e_3^2+ e_4^2+ e_5^2+ dots + e_N^2}
    ]

    整个 (F(X)) 此时是关于待优化变量的函数，每一项分别用各自的一阶泰勒展开近似，注意这里的每一项由于观测的不同，每一项都是一个不同的函数表达式，但是优化变量都是一样的。得到如下结果：

    [begin{aligned}
    frac{1}{2}|f(boldsymbol{x})+boldsymbol{J}(boldsymbol{x}) Delta boldsymbol{x}|^{2}
    &=Omega(Delta x)
    end{aligned}
    ]
    [F(X)approx Omega(Delta x)_1 + Omega(Delta x)_2+ Omega(Delta x)_3+ Omega(Delta x)_4 dots + Omega(Delta x)_N
    ]

    这里的 (Delta x) 是我们在使用基于迭代下降的方法中所选中的步长和方向，如果 (F(X)) 在 (Delta x) 为某个值时取得极小值，则 (Delta x)无论是在任何一个方向加或者减函数值都会上升，此时这个点则为极小值点，这里的叙述不太数学化，但是大家联想一下极小值的定义，应该是可以理解的，当达到该条件后，那么该点关于 (Delta x) 的导数一定为 (0) 。所以对此时的(F(X))求导并让其等于 (0) 得到：

    [mathbf{H_1Delta x } + mathbf{eta_1} + mathbf{H_2Delta x } + mathbf{eta_2} + mathbf{H_3Delta x } + mathbf{eta_3} dots + mathbf{H_NDelta x } + mathbf{eta_N} = mathbf{0}
    ]

    再将该式子变形，将关于 (Delta x) 的项都移动到左边，没有关于 (Delta x) 的移动到右边：

    [mathbf{H_1Delta x } + mathbf{H_2Delta x } + mathbf{H_3Delta x } + dots + mathbf{H_NDelta x }= - mathbf{eta_1} - mathbf{eta_2} - mathbf{eta_3} dots - mathbf{eta_N}
    ]

    其实也就是:

    [mathbf{H_1Delta x } + mathbf{H_2Delta x } + mathbf{H_3Delta x } + dots + mathbf{H_NDelta x }= mathbf{b_1} + mathbf{b_2} + mathbf{b_3} dots + mathbf{b_N}
    ]

    写成连加的形式：

    [Delta xsum_{i=1}^{N}H = sum_{i=1}^{N}b
    ]

    这里我们就通过每一项的一个具体形式来推倒出最后的 H 和 b 是怎么来的了。也就是我们经常在程序中见到的 += 操作的原理：

    H +=  J * J.transpose();
b += -J * error;

    我们再次回到曲线拟合的题目中去，待优化的变量就三个 (a,b,c) 则每一个残差项都含有这三个参数，我们称其雅可比为稠密的（虽然只有三个参数，视觉BA问题中由于相机观测的特殊性，其雅可比矩阵是稀疏的），对每一个残差向分别求雅可比，然后求和得到最终的 (H) 和 (b) ，然后求解一次 (Delta x) ，Step 一次，根据判断条件选择是否继续进行迭代。每一个残差项对于 (Delta x) 的雅可比为

    [begin{array}{l}
    J[0]_a = -x_i^2 exp(a_ex_i^2-b_ex_i-c) \
    J[1]_b = -x_i exp(a_ex_i^2-b_ex_i-c) \
    J[2]_c = -exp(a_ex_i^2-b_ex_i-c) \
    end{array}
    ]

    得到了雅可比，那么剩下的就是迭代求解即可，完整代码如下，来自14讲配套代码：

    #include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 100;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector x_data, y_data;      // 数据
  for (int i = 0; i  0 && cost >= lastCost) {
      cout = last cost: "  time_used = chrono::duration_cast<:duration>>(t2 - t1);
  cout
    • 运行结果如下：
    total cost: 3.19575e+06,                update: 0.0455771  0.078164 -0.985329           estimated params: 2.04558,-0.921836,4.01467
total cost: 376785,             update:  0.065762  0.224972 -0.962521           estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6,            update: -0.0670241   0.617616  -0.907497                estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01,            update: -0.522767   1.19192 -0.756452           estimated params: 1.52155,1.11267,1.3882
total cost: 174.853,            update: -0.537502  0.909933 -0.386395           estimated params: 0.984045,2.0226,1.00181
total cost: 102.78,             update: -0.0919666   0.147331 -0.0573675                estimated params: 0.892079,2.16994,0.944438
total cost: 101.937,            update: -0.00117081  0.00196749 -0.00081055             estimated params: 0.890908,2.1719,0.943628
total cost: 101.937,            update:   3.4312e-06 -4.28555e-06  1.08348e-06          estimated params: 0.890912,2.1719,0.943629
total cost: 101.937,            update: -2.01204e-08  2.68928e-08 -7.86602e-09          estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00440302 seconds. 
estimated abc = 0.890912, 2.1719, 0.943629
    • 和真实结果对比，这里的准确度取决于我们噪声方差的大小
    (a) (b) (c)
    Estimate (0.890912) (2.1719) (0.943629)
    Real (1) (2) (1)

    下一节我们来讨论一下视觉SLAM中的非线性优化问题的具体形式，以及其 (H) 和 (b) 的由来和构建方法。

    posted @
    2023-03-13 12:04 
    weihao_ysgs 
    阅读(2) 
    评论(0) 
    编辑 
    收藏 
    举报

文章来源于互联网：非线性优化问题基本形式概述

THE END
分享
二维码