使用C++实现最小二乘曲线拟合（使用Ceres实现）

使用C++实现最小二乘曲线拟合（使用Ceres实现）

自学视觉SLAM14讲-6.3.2，自学笔记

理论部分

详细的理论部分请看视觉SLAM14讲（第二版），从132页开始的6.3节，曲线拟合问题，第一小节6.3.1-手写高斯牛顿法，这里有非常详细的介绍，此处不再赘述！！！，此外，在下面还写了我自己的一些理解。

个人理解

$$y=e^{(a{x}^2+bx+c)}+\omega$$
那么对于第i个数据点，误差项e_i的表达式为：
$$e_i=y_i-e^{(a{x}_i^2+b{x}_i+c)}$$
那么目标函数的表达式为：
$$\frac{1}{2}\sum _{i=1}^N||e_i||$$
其中a、b、c是曲线的参数，也是待估计的变量；高斯牛顿法需要对每个待估计的变量求导数，进而可以计算雅可比矩阵：
$$\frac{\partial e_i}{\partial a}=-x_i^2{e}^{(a{x}_i^2+b{x}_i+c)}$$
$$\frac{\partial e_i}{\partial b}=-x_i{e}^{(a{x}_i^2+b{x}_i+c)}$$
$$\frac{\partial e_i}{\partial c}=-e^{(a{x}_i^2+b{x}_i+c)}$$

那么第i个样本点x_i和y_i所对应的雅可比矩阵J_i的表达式为：
$$J_i=[\frac{\partial e_i}{\partial a}\frac{\partial e_i}{\partial b}\frac{\partial e_i}{\partial c}{]}^T$$
高斯牛顿法的增量方程为：
$$(\sum _{i=1}^N\frac{1}{{\sigma }^2}{\mathbf{J}}_{\mathbf{i}}{{\mathbf{J}}_{\mathbf{i}}}^T)\Delta x_k=\sum _{i=1}^N-\frac{1}{{\sigma }^2}{\mathbf{J}}_{\mathbf{i}}{e}_i$$

在实际编程时，会有如下标记：
$${\mathbf{H}}_{\mathbf{i}}=\frac{1}{{\sigma }^2}{\mathbf{J}}_{\mathbf{i}}{{\mathbf{J}}_{\mathbf{i}}}^T$$
$${\mathbf{b}}_{\mathbf{i}}=-\frac{1}{{\sigma }^2}{\mathbf{J}}_{\mathbf{i}}{e}_i$$
成为海森矩阵，这一个3×3的矩阵。

基本概念

高斯牛顿法实现时，需要根据目标函数计算雅可比矩阵，进一步计算海森矩阵，当目标函数比较简单时，雅可比矩阵和海森矩阵的计算比较容易，但是当当待估计的参数较多，并且目标函数比较复杂时，雅可比矩阵的计算非常复杂，因此引入谷歌的Ceres库，在使用Ceres进行曲线拟合之前，有几个基础概念需要了解一下：

参数块 ：是指x₁、x₂，… ，x~n ~ 是待优化的变量；
代价函数 ：f_i ，也称为残差块，在SLAM中通常是误差项e_i;
目标函数 ：所有代价函数的平方项经过加权求和后（再除以二分之一）。

安装Ceres

先安装依赖环境，ubuntu终端输入：

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-dev

在slambook2/3rdparty/ceres-solver文件夹下打开终端并输入：

mkdir build
cd build/
cmake ..
make -j4
sudo make install

安装方法也可以查看链接，视觉slam的环境部署都在这里：
视觉SLAM十四讲-环境部署

使用Ceres进行曲线拟合的操作步骤：

1、定义每个参数块：参数块通常为平凡的向量，但是在SLAM里也可以定义成四元数、李代数这种特殊的结构。如果是向量，那么我们需要为每个参数块分配一个double数组来存储变量的值。
2、定义残差块的计算方式：也就是定义代价函数的计算方式，残差块通常关联若干个参数块，对它们进行一些自定义的计算，然后返回残差值。Ceres对它们求平方和之后，作为目标函数的值。
$$e_i=y_i-e^{(a{x}_i^2+b{x}_i+c)}$$

3、定义残差块的雅可比的计算方式：在 Ceres中，你可以使用它提供的“自动求导”功能，也可以手动指定雅可比的计算过程。如果要使用自动求导，那么残差块需要按照特定的写法书写:残差的计算过程应该是一个带模板的括号运算符。这一点我们通过例子来说明。

4、将所有的参数块和残差块加入Ceres定义的Problem对象：调用Solve函数求解即可。求解之前,我们可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认的配置。

上代码:

关键代码：

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

完整代码：

//
// Created by xiang on 18-11-19.
//

#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; // problem 对象的实例化
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
      // 误差是1个数，因此维度是1，输入是abc待估计变量，维度是3维的；
      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;
}

程序运行结果

从运行时间上看，使用高斯牛顿法比使用Ceres的自动求导法快了将近10倍！！！