状态估计之非线性优化学习

SLAM的后端优化，一般分为滤波方法与非线性优化方法，其中部分思想重叠，具有很鲜明的对比特性。在前几章中，我们学习了KF系列与PF系列，并对一部分内容进行扩展，这些方法的本质是在一步步过程中进行优化收敛，本篇将引入后端整体优化思想，在建图，轨迹跟踪，系统状态估计中应用广泛。

本篇的学习大纲是：

1. 状态估计问题与最大似然估计；
2. 线性估计的最小二乘法；
3. 非线性最小二乘法：梯度下降：最速/牛顿法；高斯-牛顿方法；列文伯格-马夸尔特方法学习；
4. ceres库的简介与应用学习。

1.状态估计问题与最大似然估计的引出

在SLAM中，我们可以通过滤波来单步进行优化状态估计的结果（马尔科夫思想），但需要知道的是，即使我们的传感器精度再高，运动方程与观测方程也只是近似成立。所以，与其假设数据符合目标方程，不如直接讨论如何在有噪声的数据中进行准确的状态估计。首先想起我们最基本的状态方程和观测方程。

直观一点讲，似然是指“在现在的位姿下，可能会产生怎样的观测数据”。由于我们能够获得观测数据，所以最大似然估计可以理解为：“在什么样的状态下，最可能产生现在观测到的数据”。这就是SLAM中最大似然估计的直观意义。

2.线性估计最小二乘法的引出

最小二乘法的目标是：求解误差的最小平方和。根据Cost Function的对应，分为了线性与非线性。这个取决于对应的残差(Residual)是线性的还是非线性的。

先从模型角度入手，我们假设某次观测为：

3.非线性最小二乘法

考虑一个简单的最小二乘问题：

4.Ceres的简介与学习

Ceres是一个广泛使用的最小二乘问题求解库，其代码基于C++实现。我们使用时只需要按照一定的步骤定义待求解的优化问题，然后运行代码就可以。Crese求解的最小二乘问题最一般的形式如下(带边界的核函数最小二乘)：

Ceres安装

代码库地址：https://link.zhihu.com/?target=https%3A//github.com/ceres-solver/ceres-solver

下载后cmake，make，make install编译安装就行。在/usr/local/lib/目录下能够看到libceres.a的库就说明安装完成。

Ceres的使用^[3]：

//代码借鉴于高翔博士的SLAM专题 ch6

#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) {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const {
    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数据
};

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));
  }

  double abc[3] = {ae, be, ce};

  // 构建最小二乘问题
  ceres::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                 // 待估计参数
    );
  }

  // 配置求解器
  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;
}

大意是使用CV的生成器随机生成100个带高斯噪声的数据，利用Ceres进行拟合。代码中涉及到的注意点(与前述工作步骤相似)有：

1. 书写了一个结构体，并在类中定义带模板参数的()运算符，形成了一个拟函数，这样直接用Ceres的对象调用该函数就可以；
2. 参数块求导部分，采用自动求导函数Auto diff，类似的选择还有数值求导Numeric diff或自行编写求导部分，出于方便，使用自动求导；
3. 优化的量的单位及维度，本篇中的误差是标量，所以单位与维度均为1，所以Auto diff中对a、b、c三个优化量的参数设置为1，3(表示单位标量1，维度3个);
4. 设定好后，在options中配置优化选项。例如，是否使用线搜索，或是置信区间，迭代步长，次数等。

Ceres的简介与学习说到这里，更多的学习可以参考文献书籍与代码。关于SLAM框架，也有许多应用，计划在后期进行学习记录。

至此，本篇的记录即将结束，再次回顾学习记录的内容：状态估计问题的优化之后端非线性优化，与单步马尔科夫式滤波(如EKF,UKF等)不同的是这种方法会对整体的模型函数进行非线性最小二乘思想的问题构建，通过导数定义的方式去寻求目标函数的极小值，来确保结果收敛。在求解极小值过程中，我们学习了在分布函数一阶/二阶泰勒展开截断的梯度下降法，分别是最速下降法与牛顿法；并推理对其概率密度函数的泰勒展开进行截断，得到了高斯牛顿方法；随后在其步长上做出优化，将置信区间的思想加入高斯牛顿法，得到列文伯格-马夸尔特方法(LM方法)。最后，简单介绍学习了SLAM中用于处理非线性优化的Ceres库。

参考大佬：SLAM算法工程师之路：状态估计之非线性优化学习 - 知乎 (zhihu.com)