Handy notes - Ceres solves curve fitting problems

illustrate

Google Ceres is a widely used least squares problem solving library. In Ceres, you only need to define the optimization problem to be solved according to certain steps, and then submit it to the solver for calculation.

step

1. Define each parameter block.
   The parameter block is usually an ordinary vector, but it can also be defined as a special structure such as a quaternion and a Lie algebra in SLAM. If it is a vector, a double array needs to be allocated for each parameter block to store the value of the variable.
2. Defines how the residual block is calculated.
   A residual block usually associates several parameter blocks, performs some custom calculations on them, and then returns the residual value. Ceres squares and sums them as the value of the objective function.
3. The residual block often also needs to define the calculation method of Jacobian.
   In Ceres, you can use the "automatic derivation" function it provides, or you can manually specify the Jacobian calculation process. If you want to use automatic differentiation, then the residual block needs to be written in a specific way: the calculation process of the residual should be a parenthesis operator with a template.
4. Add all parameter blocks and residual blocks to the Problem object defined by Ceres, and call the Solve function to solve it.
   Before solving, you can pass in some configuration information, such as the number of iterations, termination conditions, etc., or use the default configuration.

source code

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

1. Class that defines residual blocks. The method is to write a class (or structure), and define the () operator with template parameters in the class, so that the class becomes a quasi-function (Functor). This way of definition allows Ceres to call the a() method on an object of the class (such as a) just like calling a function. In fact, Ceres will pass the Jacobian matrix into this function as a type parameter, so as to realize the function of automatic derivation.
2. The double abc[3] in the program is the parameter block, and for the residual block, we construct a CURVE_FITTING_COST object for each data, and then call AddResidualBlock to add the error term to the objective function. Since optimization requires gradients, we have several options: (1) use Ceres' automatic derivation (Auto Diff); (2) use numerical derivation (Numeric Diff); (3) derive the analytical derivative form by ourselves and provide it to Ceres . Because automatic differentiation is the most convenient in coding, we use automatic differentiation.
3. Automatic differentiation requires specifying the dimensions of the error term and optimization variables. The error here is a scalar with a dimension of 1; the optimized three quantities are a, b, and c with a dimension of 3. Therefore, set the variable dimensions as 1 and 3 in the template parameter of the automatic derivation class AutoDiffCostFunction.
4. After setting the problem, call the Solve function to solve it. You can configure (very detailed) optimization options in options. For example, you can choose whether to use Line Search or Trust Region, number of iterations, step size, and so on.