非线性优化问题以及在视觉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\|<\delta\) 有 \(F\left(\mathbf{x}^{*}\right) \leq F(\mathbf{x})\)
\(\quad\)损失函数泰勒展开，假设损失函数 \(F(\mathbf{x})\) 是可导并且平滑的, 因此, 二阶泰勒展开:
这里要着重注意一下，这里的 \(\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\) 忽略泰勒展开的高阶项,损失函数变成了二次函数,可以轻易得到如下性质：
- 如果在点 \(x_s\) 处有导数为 \(0\) ,则称这个点为稳定点。
- 在点 \(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})<F(x_k) \]分两个步骤；第一，找到下降方向单位向量 \(d\) ，第二，确定下降的步长 \(a\)。
假设 \(a\) 足够的小，又因为 \(d\) 为单位向量，因此可以将 \(ad\) 看作是一个微小的变化量 \(\triangle{x}\)，我们可以对损失函数进行一阶泰勒展开：
\[F(\mathbf{x}+a \ \mathbf{d}) \approx F(\mathbf{x}) + a\mathbf{J}\mathbf{d} \]只需要寻找下降方向，满足：
\[\mathbf{Jd}<0 \]通过 line search 的方法找到下降的步长：\(a^*=argmin_{a>0} [F(x+ad)]\)
2.1 最速下降法: 适用于迭代的开始阶段
适用于迭代的开始阶段
\(\quad\) 从下降方向的条件(单位向量)可以知道: \(\mathbf{Jd=||J||}cos\theta\) ，其中 \(\theta\) 表示的是下降方向和梯度方向的夹角. 当 \(\theta = \pi\) 有:
\[\mathbf{d=\frac{-J^T}{||J||}} \]这里为什么能写成向量的内积运算,笔者在刚开始看起来还认为是两个矩阵相乘法,却直接写成了内积运算，仔细思索发现 \(d\) 其实上是一个和 \(x\) 相同维度的单位向量，其纬度为 \(n\times 1\) ，而雅可比矩阵
\(\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 x\right ) + \frac{1}{2}\mu \Delta x^T\Delta x \right \} \]其中,\(μ ≥ 0\) 为阻尼因子, $ \frac{1}{2}\mu \Delta x^T\Delta x $是惩罚项。对新的损失函数求一阶导,并令其等于 \(0\) 有:
\[\mathbf{L^`(\Delta x)} + \mu \mathbf{\Delta x} = 0 \\ (\mathbf{H}+\mu\mathbf{I})\Delta x = -\mathbf{J^T} \]2.4 Gauss-Newton 和 LM
残差函数 \(f(x)\) 为非线性函数，对其进行一阶泰勒近似有：
\[f(x+\Delta x)\approx \ell (\Delta x) \equiv f(x)+J\Delta 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<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
接下来我们关心雅可比如何计算，误差项 \(f_i(a,b,c)\) 可以写成如下形式：
\[f_i(a,b,c)=y_i-exp(a_ex_i^2+b_ex_i+c_e) \]我们知道这两项相减是绝对不可能相等的，因为在生成数据的时候加入了高斯噪声。我们这里有 \(N\) 个观测，即 \(i\in (1-100)\)，我们将其写成连加的形式
\[F(X)=\sum_{i=1}^{N}\left (y_i-exp(a_ex_i^2+b_ex_i+c_e) \right)^2 \]该式中右边就是残差项的具体形式，我们对其进行平方，防止负的残差和正的残差抵消的情况，前面我们已经说过可以将残差项通过一阶泰勒展开进行近似，然后进行平方得到每一个残差项的具体形式：
\[\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} \]\(f(x+\Delta x)\approx f(x)+J(x)\Delta x\)
此时由于某时刻的观测已知，因此误差项是一个关于 \(\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{H\Delta x} \]
即我们常见的形式：
\[\mathbf{H\Delta x=b} \]读者要注意到这里的 \(b\) 其实就是上面的 \(-\eta\)
这里我们假设残差项记为 \(\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_1\Delta x } + \mathbf{\eta_1} + \mathbf{H_2\Delta x } + \mathbf{\eta_2} + \mathbf{H_3\Delta x } + \mathbf{\eta_3} \dots + \mathbf{H_N\Delta x } + \mathbf{\eta_N} = \mathbf{0} \]再将该式子变形，将关于 \(\Delta x\) 的项都移动到左边，没有关于 \(\Delta x\) 的移动到右边：
\[\mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= - \mathbf{\eta_1} - \mathbf{\eta_2} - \mathbf{\eta_3} \dots - \mathbf{\eta_N} \]其实也就是:
\[\mathbf{H_1\Delta x } + \mathbf{H_2\Delta x } + \mathbf{H_3\Delta x } + \dots + \mathbf{H_N\Delta x }= \mathbf{b_1} + \mathbf{b_2} + \mathbf{b_3} \dots + \mathbf{b_N} \]写成连加的形式：
\[\Delta x\sum_{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 <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
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<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T W^{-1} J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = yi - exp(ae * xi * xi + be * xi + ce);
Vector3d J; // 雅可比矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc
H += J * J.transpose();
b += -J * error;
cost += error * error;
}
// 求解线性方程 Hx=b
Vector3d dx = H.ldlt().solve(b);
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) {
cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
break;
}
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
"\t\testimated params: " << ae << "," << be << "," << ce << endl;
}
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
- 运行结果如下：
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\) 的由来和构建方法。
标签：非线性,mathbf,boldsymbol,cost,概述,Delta,xi,frac,优化 From： https://www.cnblogs.com/weihao-ysgs/p/basic-form-of-nonlinear-optimization.html