Ceres入门(二)曲线拟合与BA求解

原创

已于 2022-01-28 10:38:18 修改 · 1.3k 阅读

6 ·

CC 4.0 BY-SA版权

文章标签：

#slam #算法

于 2022-01-26 21:06:59 首次发布

本文通过曲线拟合和捆绑调整两个案例，介绍了Ceres求解器的基本使用方法。包括构建代价函数、优化问题及配置求解器参数等内容。

在一篇博客中，通过分析helloword的自动求导和节写求导简单例子，了解了Ceres的基本流程。本片博客在上一片基础之上，以高博十四讲内容为基础，分析Ceres两个使用案例

一、曲线拟合

1、问题描述

其中a,b,c为待估计的参数，w为噪声。在程序里利用模型生成x，y的数据，在给数据添加服从高斯分布的噪声。之后用ceres优化求解参数a,b,c。

2、求解代码

代码部分仍然与上一篇博客类似，分为三个部分

(1)、第一部分：构建cost fuction，即代价函数

struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

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

template<typename T>用法参考：

template＜typename T＞简单用法_realjc的博客-优快云博客_template<typename>

利用operator()使得该结构体成为一个拟函数。这种方法定义使得Ceres可以像调用函数一样对该结构体的对象进行调用例如(对象为a)a<double>()方法。Cerse会把雅可比矩阵作为类型参数传入此函数。从而实现自动求导。

(2)第二部分：通过代价函数构建待求解的优化问题

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

残差函数维度为1，参数维度为3。

(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);  // 开始优化

调用Solve函数进行求解，可以在options中配置。例如，可以选择使用Line Search或者Trust R