Ceres usage notes

Article directory

Part.I Introduction
- Chapter.I Preliminary knowledge
- Chapter II Concept Understanding
Part.II Simple to use
- Introduction to the main functions in Chap.I Ceres
- Chap.II A simple example
Reference

Part.I Introduction

Ceres ¹ is an open source C++ general nonlinear optimization library developed by Google. It is jointly with g2o as the most widely used visual SLAM at present. Optimization algorithm library. This blog post briefly introduces the notes I took while using Ceres. I welcome criticism and correction for any inappropriateness or deficiencies!

Insert image description here

Chapter.I Preliminary knowledge

Before using Ceres, you need to have a certain understanding of the concept of graph optimization.

Chapter II Concept Understanding

Here are some concepts involved in using Ceres:

Cost function: CostFunction is the objective formula (objective function) of optimization. It contains the dimension information of the parameter module, and uses functors internally to define the calculation method of the error function. Equivalent to $V^TPV=min in least squares$
Loss function: LossFunction is used to handle situations where parameters contain outliers to avoid the impact of erroneous measurements on estimation. Commonly used parameters include HuberLoss, CauchyLoss, etc.
Marginalization: The object of marginalization is the oldest frame or the next newest frame in the sliding window. Some older or unsatisfactory frames in the sliding window are eliminated, so marginalization is also described. It is the process of decomposing the joint probability distribution into marginal probability distribution and conditional probability distribution (that is, the process of using Schur to supplement and reduce the optimization parameters). Marginalization is to encapsulate the lost frames into prior information and add it to the new nonlinear optimization problem as part of the error.

Part.II Simple to use

Introduction to the main functions in Chap.I Ceres

This part mainly comes from the summary of seniors on the Internet ². The author has made some adjustments based on his own understanding. Optimization in Ceres requires two steps:

Constructing an optimization problem: including constructing an optimized residual function CostFunction and adding a residual block after each data acquisition (using the AddResidualBlock method of the Problem class, etc.)
Solve optimization problems: set how to solve the equation, whether the solution process is output, etc. (the Solve method will be called)

CostFunction class: Constructing optimized residual functions

Like other nonlinear optimization toolkits, the performance of ceres relies heavily on the accuracy and efficiency of derivative calculations. This part of the work is called CostFunction in ceres. Ceres provides many CostFunction templates, the more commonly used ones include the following three:

Automatic derivative: AutoDiffCostFunction, ceres determines the calculation method of the derivative by itself, the most commonly used derivation method.
Numerical derivative: NumericDiffCostFunction, the numerical solution form of the derivative is manually written by the user. Usually, the calculation of the residual function uses a library function that cannot be directly called, resulting in calling the AutoDiffCostFunction class construction. However, the accuracy and calculation efficiency of manual writing are not as good as the template class. Therefore, it is not officially recommended to use this method unless absolutely necessary.
Analytical derivative: SizedCostFunction, used when the derivative exists in a closed analytic form, which can be written by yourself based on the SizedCostFunction base class; however, due to the need to manage the residuals and Jacobian matrices by yourself, unless the closed solution has obvious accuracy and efficiency advantages, otherwise the same Not recommended for use.

Problem class: Construct optimization problems

Relevant methods for constructing least squares problems are included in the Ceres::Problem class, and the member functions involved mainly include:

Problem::AddResidualBlock(): Pass the residual module information to the Problem class. The parameters passed mainly include: cost function module (CostFunction *cost_function), loss function module (LossFunction *loss_function ) and parameter modules (const vector<double *> parameter_blocks).
Problem::AddParameterBlock(): Explicitly pass the parameter module to the Problem. In fact, the user has implicitly passed the parameter module to the Problem before calling AddParameterBlock(). However, in some cases, the user needs to explicitly pass the parameter module to the Problem. Problem transfer parameter module, for example, if the optimization parameters need to be re-parameterized (because the optimization parameters have changed at this time), then you need to use AddParameterBlock() to explicitly transfer the parameter module to Problem.
Problem::SetParameterBlockConstant(): The corresponding parameter module settings remain unchanged during the optimization process.
Problem::SetParameterBlockVariable(): Set the corresponding parameter module to be variable during the optimization process
Problem::SetParameterUpperBound() andProblem::SetParameterLowerBound(): Set optimization upper and lower bounds
Problem::Evaluate(): This function, immediately after parameter assignment, solves the Problem at the given parameter position and gives the cost, gradient and Jacobian matrix at the current position;

Solver class: Solving optimization problems

ceres::SolveThe function is the core function of Ceres to solve the least squares problem. The function prototype is as follows:

void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary)

The parameters passed in by the function include:

Solver::Options: Solving option, which is the core of Ceres solution. All solver behaviors including elimination order, decomposition method, convergence accuracy, etc. are controlled by Solver::Options.
Solver::Summary: Solution report is only used to store relevant information during the solution process and does not affect solver performance.
Problem: Solve the problem, that is, the optimization problem described above.

Solver::Options

Solver::Options contains a wide variety of parameters, and most of them use Ceres' default settings. Here are some commonly used or important parameters.

minimizer_type: Iterative solution method, optional linear search method (LINEAR_SEARCH), trust region method (TRUST_REGION), the default is TRUST_REGION method; since in most cases we will choose the LM or DOGLEG method, this option generally uses the default value directly;
trust_region_strategy_type: trust region policy, optional LEVENBERG_MARQUARDT or DOGLEG, default is LEVENBERG_MARQUARDT, no Gauss-Newton option; < /span>
linear_solver_type: The solver type used to solve the system of linear equations in the trust region method, the default is DENSE_QR;
linear_solver_ordering: The elimination order of the linear equation solver, the default is NULL, that is, Ceres determines the elimination order by itself; in applications that have special requirements for the elimination order, typically represented by BA, the elimination can be set through the member function reset. Meta-ordering, explained in detail later;
min_linear_solver_iteration / max_linear_solver_iteration: The minimum/maximum number of iterations of the linear solver, the default is 0/500, generally does not need to be changed;
max_num_iterations: The maximum number of iterations of the solver;
max_solver_time_in_seconds: The maximum number of seconds the solver can run;
num_threads: The number of threads used by Ceres when solving. In the old version of Ceres, there was also a thread setting option num_linear_solver_threads for the linear solver. This option has been canceled in the latest version of Ceres; although in order to ensure the compatibility of the program, users can still Set this parameter, but Ceres will automatically ignore this parameter, which has no practical significance;
minimizer_progress_to_stdout: Whether to output the optimization process information to the terminal. The default selection is false, that is, depending on the vlog setting level, only STDERR Output error information; if set to true, all information about the optimization process will be output to the running terminal of the program. The parameters output will be different depending on the optimization method set.

In practical applications, among the above parameters, the ones that have the greatest impact on the final solution performance are the linear equation solver typelinear_solver_type and the number of threadsnum_threads. If it is found that the last If the solution accuracy or solution efficiency cannot meet the requirements, you should first try to replace these two parameters.

Chap.II A simple example

Based on the above prior knowledge, let’s use Ceres to solve a simple problem ³.

Problem: A certain two-dimensional curve is known $y=e^{ax^2+bx+c}$ Top 1000 points, number of requests $a, b, c$ 目值.
Idea: First construct the cost function structure, and then generate evenly distributed between [0,1], and put these Substitute the curve to be fitted (we let xx $a = 3, b = 2, c = 1$ ) In the formula, we gety_tmp. After adding random noise with a variance of 1, we gety, so that 1000 (x,y) such data points are obtained; use these data points to find parameters $a, b, c$ 目值， $a, b, c$ given $3, 2, 1$ The closer it is, the more accurate the result will be.
ps: To put it bluntly, the process of obtaining data points above is a process of simulating observation. In the actual process of solving the problem, the data points must be obtained by observation; we do not know in advance $a, b, c$ 目值.

First construct the cost function:

//构建代价函数结构体，abc为待优化参数，residual为残差。
struct CURVE_FITTING_COST
{
    
    
    CURVE_FITTING_COST(double x, double y) :_x(x), _y(y) {
    
    }
    template <typename T>
    bool operator()(const T* const abc, T* residual)const
    {
    
    
        // y = exp(ax^2 + bx + c)
        residual[0] = _y - ceres::exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        return true;
    }
    const double _x, _y;
};

Then it is necessary to simulate the observation to generate data points, construct the optimization problem, and solve the optimization problem. The function is as follows:

int fitting_curve(double a, double b, double c)
{
    
    
#pragma region 生成数据，模拟观测
    //参数初始化设置，abc初始化为0，白噪声方差为1（使用 OpenCV 的随机数产生器）。
    RNG rng;
    double w = 1;
    double abc[3] = {
    
     0,0,0 };

    //生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
    vector<double> x_data, y_data;
    for (int i = 0; i < 1000; i++)
    {
    
    
        double x = i / 1000.0;
        x_data.push_back(x);
        y_data.push_back(std::exp(a * x * x + b * x + c) + rng.gaussian(w));
    }
#pragma endregion

#pragma region 构建优化问题
    //反复使用 AddResidualBlock 方法（逐个散点，反复1000次）
    //将每个点的残差累计求和构建最小二乘优化式
    //不使用核函数，待优化参数是 abc
    ceres::Problem problem;
    for (int i = 0; i < 1000; i++)
    {
    
    
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
                new CURVE_FITTING_COST(x_data[i], y_data[i])
                ),
            nullptr,
            abc
        );
    }
#pragma endregion

#pragma region 求解优化问题
    //配置求解器并求解，输出结果
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    cout << "a= " << abc[0] << endl;
    cout << "b= " << abc[1] << endl;
    cout << "c= " << abc[2] << endl;
#pragma endregion
    return 0;
}

The result is as follows:

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  5.277388e+06    0.00e+00    5.58e+04   0.00e+00   0.00e+00  1.00e+04        0    2.05e-04    3.98e-04
   1  4.287886e+238   -4.29e+238    0.00e+00   7.39e+02  -8.79e+231  5.00e+03        1    3.15e-04    7.20e-03
   2  1.094203e+238   -1.09e+238    0.00e+00   7.32e+02  -2.24e+231  1.25e+03        1    1.98e-04    7.68e-03
   3  5.129910e+234   -5.13e+234    0.00e+00   6.96e+02  -1.05e+228  1.56e+02        1    1.43e-04    8.13e-03
   4  1.420558e+215   -1.42e+215    0.00e+00   4.91e+02  -2.97e+208  9.77e+00        1    1.03e-04    8.65e-03
   5  9.607928e+166   -9.61e+166    0.00e+00   1.85e+02  -2.23e+160  3.05e-01        1    9.73e-05    9.17e-03
   6  7.192680e+60   -7.19e+60    0.00e+00   4.59e+01  -2.94e+54  4.77e-03        1    1.16e-04    9.62e-03
   7  5.061060e+06    2.16e+05    2.68e+05   1.21e+00   2.52e+00  1.43e-02        1    2.55e-04    1.03e-02
   8  4.342234e+06    7.19e+05    9.34e+05   8.84e-01   2.08e+00  4.29e-02        1    2.29e-04    1.09e-02
   9  2.876001e+06    1.47e+06    2.06e+06   6.42e-01   1.66e+00  1.29e-01        1    2.41e-04    1.15e-02
  10  1.018645e+06    1.86e+06    2.58e+06   4.76e-01   1.38e+00  3.86e-01        1    2.55e-04    1.22e-02
  11  1.357731e+05    8.83e+05    1.30e+06   2.56e-01   1.13e+00  1.16e+00        1    3.85e-04    1.27e-02
  12  2.142986e+04    1.14e+05    2.71e+05   8.60e-02   1.03e+00  3.48e+00        1    2.86e-04    1.34e-02
  13  1.636436e+04    5.07e+03    5.94e+04   3.01e-02   1.01e+00  1.04e+01        1    2.83e-04    1.40e-02
  14  1.270381e+04    3.66e+03    3.96e+04   6.21e-02   9.96e-01  3.13e+01        1    3.35e-04    1.46e-02
  15  6.723500e+03    5.98e+03    2.68e+04   1.30e-01   9.89e-01  9.39e+01        1    2.57e-04    1.50e-02
  16  1.900795e+03    4.82e+03    1.24e+04   1.76e-01   9.90e-01  2.82e+02        1    3.14e-04    1.55e-02
  17  5.933860e+02    1.31e+03    3.45e+03   1.23e-01   9.96e-01  8.45e+02        1    2.72e-04    1.59e-02
  18  5.089437e+02    8.44e+01    3.46e+02   3.77e-02   1.00e+00  2.53e+03        1    2.34e-04    1.62e-02
  19  5.071157e+02    1.83e+00    4.47e+01   1.63e-02   1.00e+00  7.60e+03        1    2.30e-04    1.68e-02
  20  5.056467e+02    1.47e+00    3.03e+01   3.13e-02   1.00e+00  2.28e+04        1    2.50e-04    1.73e-02
  21  5.046313e+02    1.02e+00    1.23e+01   3.82e-02   1.00e+00  6.84e+04        1    2.77e-04    1.78e-02
  22  5.044403e+02    1.91e-01    2.23e+00   2.11e-02   9.99e-01  2.05e+05        1    2.33e-04    1.83e-02
  23  5.044338e+02    6.48e-03    1.38e-01   4.35e-03   9.98e-01  6.16e+05        1    2.32e-04    1.88e-02
a= 3.01325
b= 1.97599
c= 1.01113

Although the sparrow is small, it has all the internal organs. You can see that the author will $a = 3, b = 2, c = 1$ The initial values are all assigned to 0, and a total of 23 iterations are performed. The cost ranges from $10^{238}$ Quantity has arrived $10^{2}$ magnitude, finally find out $a = 3.01325, b = 1.97599, c = 1.01113$ , given $a = 3, b = 2, c = 1$ Very close.

It is worth noting that the code in this calculation example uses opencv. For header file references, please refer to ³. The author simply encapsulates it For a moment.

Combined with the above calculation example, the general flow chart of Ceres solving optimization problems is drawn as follows: