视觉SLAM十四讲笔记第四讲第二部分非线性优化

最新推荐文章于 2024-09-22 13:49:16 发布

qq_57225906

最新推荐文章于 2024-09-22 13:49:16 发布

阅读量666

点赞数 17

文章标签：笔记算法人工智能 opencv 计算机视觉数码相机

本文链接：https://blog.csdn.net/qq_57225906/article/details/138726097

版权

引言

我们简要回顾一下前几讲的内容，第在一讲中，我们初识了SLAM，讨论了SLAM中的运动方程和观测方程。

第二讲中，我们学习了相机位姿的表达方式。第三讲中我们介绍了李群和李代数，以及相关的数学知识。第四讲的第一部分我们介绍了相机的观测模型。这些知识都是用于表达相机运动方程和观测方程的工具。

但是在实际中，因为有噪声，两个方程是无法完全成立的，我们要做的就是尽可能使得运动方程和观测方程贴近实际情况。为了实现精准定位，在这讲中我们要学习状态估计问题以及非线性优化相关部分的知识。（这讲要求对非线性最小二乘有较好的理解）

状态估计

现在的问题是如何通过传感器数据u和z，估计位姿x和路标y。解决问题的方法分为两种，一是批量状态估计问题，二是递归状态估计问题。

批量状态估计问题就是一次给定一段时间中若干个时间采集的所有数据,希望通过所有的这些数据去推断位姿x和路标y。（笼统来说，就是假设一次性给定你十张图片，现在根据这十张图片去推断采集这十张图片时，对应的相机的位姿是什么样的，以及这十张图片中的点在什么地方）。因为一次给定了所有信息，所以称为批量式的。

与之相对应的称为增量式的。增量式就是首先以当前时刻的估计状态为基础，后续在根据新的时刻产生的数据，对状态进行更新。

下面我们分别介绍这两种方式。

批量状态估计问题：

考虑1-N的所有时刻，并假设有M个路标点，定义所有时刻机器人的位姿和路标点坐标为：用u表示所有时刻的输入，z表示所有时刻的观测数据。现在进行状态估计，就是已知输入数据u和观测数据z的条件下，求状态x和y的条件概率分布。

现在我们不考虑控制输入，仅考虑观测方程带来的数据，这个问题被称为SFM。即如何从许多图像中重建三维空间结构。

利用贝叶斯法则，可以得到：

因为 $P(x,y\mid z,u)$ 条件分布很难求解，为了得到最优状态估计,我们可以求解使后验概率最大时的状态（最大后验概率估计：似然概率乘上先验概率的最大化，意思就是求解在怎样的x，y下拿到的相机数据和实际数据是最接近的）。若不知道先验部分信息，可以进行最大似然估计（MLE：最大化x，y使得这个状态下的数据最接近实际数据）。

最小二乘的引出

上面我们完成了状态估计问题的数学建模，现在我们求解上述概率，这里我们引出最小二乘。

现在考虑某一次观测，在 $x_{k}$ 处对 $y_{j}$ 进行观测，产生数据 $z_{k,j}$ 由于噪声会产生影响且噪声是高斯的，所以这里由 $x_{k}$ 和 $y_{j}$ 推测 $z_{k,j}$ 满足高斯分布，高斯分布的均值（期望）就是由x、y推出理想的z值，方差就是噪声。

现在的问题就是如何通过 $z_{k,j}$ $z_{k,j}$ 去推出x和y进行最大似然估计。

一般的高斯分布形式是这样的，由均值和方差描述：
现在我们要求最大似然概率，只需要让最大似然函数的负对数最小，高斯分布的负对数形式如下：

因为前面一项和x无关，舍去。综合上述数学模型，我们可以得到求解最大似然估计实际上就是求解下式。下式等价于最小化噪声的二次型，这个二次型称为马氏距离。

所以这里我们将状态最大似然估计问题变成了最小二乘问题。在此基础上，我们讨论批量估计问题：

我们求的最大似然估计是使一段时间内所有运动和观测的似然乘积最大的状态。转成负对数的形式：

这里就构成了最小二乘问题，目的就是找到使这些二次型和为极小值时的状态，这是典型的非线性优化问题。现在我们来讨论非线性最小二乘的解法。

非线性最小二乘

从最简单的问题开始考虑，当要求解的函数很简单的时候，可以直接求导，得到极值点和鞍点然后再比较这些点即可。当要求解的函数很复杂，通过求导方式无法解决的时候，可以使用迭代的方式求解。

迭代方式步骤：
1、给定某个初始值x0.
2、对于第k次迭代，寻求一个增量 $\Delta x_{k}$ ，使得 $\begin{Vmatrix} f(x_{k}+\Delta x_{k})) \end{Vmatrix}_{2}^{2}$ 达到极小值。
3、若 $\Delta x_{k}$ 足够小，则停止
4、否则，令 $x_{k+1}=x_{k}+$ $\Delta x_{k}$ ，返回第二步。

第二步中如何确定 $\Delta x_{k}$ 增量是问题的关键。确定增量的方法有很多，比如在函数某一点进行泰勒展开：这里的J为向量x的一阶导数，H为向量x的二阶导数，分别称为雅可比矩阵和海森矩阵。我们利用迭代方法和线性化方法去替代求导方法，本质上仍是求解：所以我们求解的增量一定要让导函数的值向0的方向靠近。

方法一：这里可以只保留第一项： $\underset{\Delta x}{min}\begin{Vmatrix} f(x)) \end{Vmatrix}_{2}^{2}+J\Delta x$ ,取增量为一阶导数反向的梯度，并通过步长 $\lambda$ 来调节：这样的方式称为一阶下降法（最速下降法）。

方法二：保留二阶梯度，对增量 $\Delta x_{k}$ 求导，令其为0：

这个方程称为增量方程。这种方法被称为二阶梯度法（牛顿法）。方法一和方法二虽然都很直观，但是都有缺点，最速下降法需要迭代很多次，牛顿法需要计算海森矩阵，很复杂。

方法三： Gauss-Newton（GN）高斯牛顿法：

首先对函数f进行一阶泰勒展开：将一阶泰勒展开带入F(x)中去，如下：再对增量求一阶导数并令其为0：

称这个方程为高斯牛顿方程。可以看到高斯牛顿方法不用计算复杂的海森矩阵，而是通过雅可比矩阵计算来近似代替海森矩阵。这样既用到了二阶梯度信息，又回避了海森矩阵的计算。总结下高斯牛顿的计算步骤：

1、给定初始值x0；
2、对于第k次迭代，求出当前的误差项f(xk)和雅可比矩阵J(xk);
3、求解增量方程H $\Delta x_{k}$ =g
4、若 $\Delta x_{k}$ 足够小则停止迭代，否则重复2，3步骤

高斯牛顿法的缺点就是求解增量方程的时候，需要求H的逆矩阵，但是我们无法保证H矩阵是可逆的。

方法四：Levenberg-Marquadt（阻尼牛顿法）

L-M方法就是在高斯牛顿方法迭代过程中，加上信赖区域，认为近似只在区域内可靠，这里用 $\rho$ 来描述近似程度好坏：L-M方法如下：在信赖区域内优化，利用拉格朗日乘子转化为无约束：再按照高斯牛顿的方法展开，得到增量方程，求解即可。

阻尼牛顿法相比于高斯牛顿法，保证了增量方程的正定性，认为近似只在一个范围内成立，如果近似指标不好则缩小近似范围。从增量方程的角度上看，既有一阶梯度又有二阶梯度，通过 $\lambda$ 来控制两边权重。

实践部分：利用Ceres库 and g2o库求解曲线拟合问题

问题：已知一个曲线方程 $y=exp(ax^{2}+bx+c)+w$ ，w是噪声，现给定一组受噪声影响的x、y，目的是通过这一组x、y去推断曲线方程中的参数。这个问题可以当作最大似然问题求解：先定义误差，再使误差最小化。现在通过实践部分演示如何通过Ceres库和g2o库来解决上述问题。

Ceres：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {} //定义了结构体CURVE_FITTING_COST，用于表示曲线拟合的代价函数，在结构体的构造函数中，初始化了x、y值

  // 残差的计算
  template<typename T>
  bool operator()( //重载函数调用运算符operator()，用于计算残差，T是模板参数
    const T *const abc, // 模型参数，有3维
    T *residual) const { //residual是输出的残差
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c) 
    return true;
  }

  const double _x, _y;    // 定义了两个成员变量_x,_y，用于存储数据点x值和y值
};

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;      // 定义了两个向量 x_data 和 y_data，用来存储数据点的 x 值和 y 值
  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));
  } //通过循环生成了N个数据点，其中x值均匀分布在0-1之间，y值则是根据真实参数值以及高斯分布噪声生成

  double abc[3] = {ae, be, ce}; //定义了数组abc用于存储待估计的参数

  // 构建最小二乘问题
  ceres::Problem problem; //创建了优化问题对象problem
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项，使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
        new CURVE_FITTING_COST(x_data[i], y_data[i])
      ),
      nullptr,            // 核函数，这里不使用，为空
      abc                 // 待估计参数
    );
  }//循环遍历了每个数据点，并将其添加到优化问题中，这里使用了自动微分技术AutoDiffCostFunction来计算残差，CURVE_FITTING_COST是代价函数模型，1是残差维度，3是参数维度

  // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 输出到cout

  ceres::Solver::Summary summary;                // 优化信息
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  ceres::Solve(options, &problem, &summary);  // 开始优化
  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 << summary.BriefReport() << endl;
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}
                                                                                                                                                                                                         1,1          全部

实验结果如下：