Ceres 使用笔记

文章目录

Part.I Introduction
- Chap.I 预备知识
- Chap.II 概念理解
Part.II 简单使用
- Chap.I Ceres 中主要函数简介
- Chap.II 一个简单的实例
Reference

Part.I Introduction

Ceres ¹ 是由 Google 开发的开源 C++ 通用非线性优化库，与 g2o 并列为目前视觉 SLAM 中应用最广泛的优化算法库。本篇博文简要介绍笔者使用 Ceres 过程中所做笔记，不当与不足之处还望批评指正！

在这里插入图片描述

Chap.I 预备知识

在使用 Ceres 之前，需要对图优化的概念有一定的理解。

Chap.II 概念理解

下面是使用 Ceres 过程中会涉及的一些概念：

代价函数：CostFunction 也就是寻优的目标式（目标函数）。它包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。相当于最小二乘中的 $V^TPV=min$
损失函数：LossFunction 用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括 HuberLoss、CauchyLoss 等。
边缘化：marginalization，边缘化的对象是滑动窗口中的最老帧或者次新帧，将滑窗内的某些较旧或者不满足要求的帧剔除，所以边缘化也被描述为将联合概率分布分解为边缘概率分布和条件概率分布的过程(就是利用 Schur 补减少优化参数的过程)。边缘化是将丢掉的帧封装成先验信息，加入到新的非线性优化问题中作为一部分误差。

Part.II 简单使用

Chap.I Ceres 中主要函数简介

这部分主要来源于网络中前辈们的总结 ²，笔者根据自己的理解进行了一些调整。Ceres 中的优化需要两步：

构建优化问题：包括构建优化的残差函数 CostFunction、在每次获取到数据后添加残差块（用到 Problem 类 AddResidualBlock 方法等）
求解优化问题：设置方程怎么求解，求解过程是否输出等（会调用 Solve 方法）

CostFunction 类：构建优化的残差函数

与其他非线性优化工具包一样，ceres 的性能很大程度上依赖于导数计算的精度和效率。这部分工作在 ceres 中称为 CostFunction，ceres 提供了许多种 CostFunction 模板，较为常用的包括以下三种：

自动导数：AutoDiffCostFunction，由 ceres 自行决定导数的计算方式，最常用的求导方式。
数值导数：NumericDiffCostFunction，由用户手动编写导数的数值求解形式，通常在残差函数的计算使用无法直接调用的库函数，导致调用AutoDiffCostFunction 类构建时使用；但手动编写的精度和计算效率不如模板类，因此不到不得已，官方并不建议使用该方法。
解析导数：SizedCostFunction，当导数存在闭合解析形式时使用，用于可基于 SizedCostFunction 基类自行编写；但由于需要自行管理残差和雅克比矩阵，除非闭合解具有具有明显的精度和效率优势，否则同样不建议使用。

Problem 类：构建优化问题

构建最小二乘问题的相关方法均包含在 Ceres::Problem 类中，涉及的成员函数主要包括：

Problem::AddResidualBlock()：向 Problem 类传递残差模块的信息，传递的参数主要包括：代价函数模块（CostFunction *cost_function）、损失函数模块（LossFunction *loss_function）和参数模块（const vector<double *> parameter_blocks）。
Problem::AddParameterBlock()：显式地向 Problem 传递参数模块，其实用户在调用 AddParameterBlock()以前已经隐式地向 Problem 传递了参数模块，但是在一些情况下需要用户显示地向 Problem 传递参数模块，比如需要对优化参数进行重新参数化的情况（因为这个时候优化的参数已经变了），这个时候就需要使用 AddParameterBlock() 显示地向 Problem 传递参数模块。
Problem::SetParameterBlockConstant()：设定对应的参数模块在优化过程中保持不变。
Problem::SetParameterBlockVariable()：设定对应的参数模块在优化过程中可变
Problem::SetParameterUpperBound()和Problem::SetParameterLowerBound()：设定优化上下界
Problem::Evaluate()：该函数紧跟在参数赋值后，在给定的参数位置求解Problem，给出当前位置处的 cost、梯度以及 Jacobian 矩阵；

Solver 类：求解优化问题

ceres::Solve函数是 Ceres 求解最小二乘问题的核心函数，函数原型如下：

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

函数传入的参数包括：

Solver::Options：求解选项，是 Ceres 求解的核心，包括消元顺序、分解方法、收敛精度等在内的求解器所有行为均由 Solver::Options 控制。
Solver::Summary：求解报告，只用于存储求解过程中的相关信息，并不影响求解器性能。
Problem：求解问题，即上面所述的优化问题。

Solver::Options

Solver::Options 中含有的参数种类繁多，绝大多数参数都会使用 Ceres 的默认设置，这里列出一些常用或较为重要的参数。

minimizer_type：迭代求解方法，可选线性搜索方法（LINEAR_SEARCH）、信赖域方法（TRUST_REGION），默认为TRUST_REGION 方法；由于大多数情况我们都会选择 LM 或 DOGLEG 方法，该选项一般直接采用默认值；
trust_region_strategy_type：信赖域策略，可选LEVENBERG_MARQUARDT或DOGLEG，默认为LEVENBERG_MARQUARDT，没有高斯牛顿选项；
linear_solver_type：信赖域方法中求解线性方程组所使用的求解器类型，默认为 DENSE_QR；
linear_solver_ordering：线性方程求解器的消元顺序，默认为 NULL，即由 Ceres 自行决定消元顺序；在以 BA 为典型代表的，对消元顺序有特殊要求的应用中，可以通过成员函数 reset 设定消元顺序，稍后将详细说明；
min_linear_solver_iteration / max_linear_solver_iteration：线性求解器的最小/最大迭代次数，默认为 0/500，一般不需要更改；
max_num_iterations：求解器的最大迭代次数；
max_solver_time_in_seconds：求解器的最大运行秒数；
num_threads：Ceres 求解时使用的线程数，在老版本的 Ceres 中还有一个针对线性求解器的线程设置选项 num_linear_solver_threads，最新版本的 Ceres 中该选项已被取消；虽然为了保证程序的兼容性，用户依旧可以设置该参数，但 Ceres 会自动忽略该参数，并没有实际意义；
minimizer_progress_to_stdout：是否向终端输出优化过程信息，该选型默认为 false，即根据 vlog 设置等级的不同，只会在向 STDERR 中输出错误信息；若设置为 true 则会向程序的运行终端输出优化过程的所有信息，根据所设置优化方法的不同，输出的参数亦不同。

在实际应用中，上述参数中对最终求解性能最大的就是线性方程求解器类型 linear_solver_type 和线程数 num_threads，如果发现最后的求解精度或求解效率不能满足要求，应首先尝试更换这两个参数。

Chap.II 一个简单的实例

基于上述先验知识，我们一起来用 Ceres 求解一个简单的问题 ³。

问题：已知某二维曲线 $y=e^{ax^2+bx+c}$ 上 1000 个点的坐标（有噪声），求参数 $a, b, c$ 的值。
思路：首先构造代价函数结构体，然后生成在[0,1] 之间均匀分布的 x，把这些 x 代入待拟合曲线（我们就令 $a = 3, b = 2, c = 1$ ）式子中，得到 y_tmp，在加上方差为1的随机噪声，得到 y，如此便得到 1000 个 (x,y) 这样的数据点；用这些数据点来求参数 $a, b, c$ 的值， $a, b, c$ 与 $3, 2, 1$ 越接近说明得到的结果越准确。
ps：上面我们得到数据点的过程说白了就是一个模拟观测的过程，实际处理问题过程中，数据点肯定是由观测得到的；我们事先并不知道 $a, b, c$ 的值。

首先构造代价函数：

//构建代价函数结构体，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;
};

然后就是模拟观测生成数据点、构造优化问题、求解优化问题，函数如下：

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次）//将每个点的残差累计求和构建最小二乘优化式//不使用核函数，待优化参数是 abcceres::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 endregionreturn 0;
}

结果如下：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time0  5.277388e+06    0.00e+00    5.58e+04   0.00e+00   0.00e+00  1.00e+04        0    2.05e-04    3.98e-041  4.287886e+238   -4.29e+238    0.00e+00   7.39e+02  -8.79e+231  5.00e+03        1    3.15e-04    7.20e-032  1.094203e+238   -1.09e+238    0.00e+00   7.32e+02  -2.24e+231  1.25e+03        1    1.98e-04    7.68e-033  5.129910e+234   -5.13e+234    0.00e+00   6.96e+02  -1.05e+228  1.56e+02        1    1.43e-04    8.13e-034  1.420558e+215   -1.42e+215    0.00e+00   4.91e+02  -2.97e+208  9.77e+00        1    1.03e-04    8.65e-035  9.607928e+166   -9.61e+166    0.00e+00   1.85e+02  -2.23e+160  3.05e-01        1    9.73e-05    9.17e-036  7.192680e+60   -7.19e+60    0.00e+00   4.59e+01  -2.94e+54  4.77e-03        1    1.16e-04    9.62e-037  5.061060e+06    2.16e+05    2.68e+05   1.21e+00   2.52e+00  1.43e-02        1    2.55e-04    1.03e-028  4.342234e+06    7.19e+05    9.34e+05   8.84e-01   2.08e+00  4.29e-02        1    2.29e-04    1.09e-029  2.876001e+06    1.47e+06    2.06e+06   6.42e-01   1.66e+00  1.29e-01        1    2.41e-04    1.15e-0210  1.018645e+06    1.86e+06    2.58e+06   4.76e-01   1.38e+00  3.86e-01        1    2.55e-04    1.22e-0211  1.357731e+05    8.83e+05    1.30e+06   2.56e-01   1.13e+00  1.16e+00        1    3.85e-04    1.27e-0212  2.142986e+04    1.14e+05    2.71e+05   8.60e-02   1.03e+00  3.48e+00        1    2.86e-04    1.34e-0213  1.636436e+04    5.07e+03    5.94e+04   3.01e-02   1.01e+00  1.04e+01        1    2.83e-04    1.40e-0214  1.270381e+04    3.66e+03    3.96e+04   6.21e-02   9.96e-01  3.13e+01        1    3.35e-04    1.46e-0215  6.723500e+03    5.98e+03    2.68e+04   1.30e-01   9.89e-01  9.39e+01        1    2.57e-04    1.50e-0216  1.900795e+03    4.82e+03    1.24e+04   1.76e-01   9.90e-01  2.82e+02        1    3.14e-04    1.55e-0217  5.933860e+02    1.31e+03    3.45e+03   1.23e-01   9.96e-01  8.45e+02        1    2.72e-04    1.59e-0218  5.089437e+02    8.44e+01    3.46e+02   3.77e-02   1.00e+00  2.53e+03        1    2.34e-04    1.62e-0219  5.071157e+02    1.83e+00    4.47e+01   1.63e-02   1.00e+00  7.60e+03        1    2.30e-04    1.68e-0220  5.056467e+02    1.47e+00    3.03e+01   3.13e-02   1.00e+00  2.28e+04        1    2.50e-04    1.73e-0221  5.046313e+02    1.02e+00    1.23e+01   3.82e-02   1.00e+00  6.84e+04        1    2.77e-04    1.78e-0222  5.044403e+02    1.91e-01    2.23e+00   2.11e-02   9.99e-01  2.05e+05        1    2.33e-04    1.83e-0223  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

麻雀虽小，五脏俱全。可以看到笔者将 $a = 3, b = 2, c = 1$ 初值都赋为了 0，共迭代了 23 次，代价从 $10^{238}$ 量级降到了 $10^{2}$ 量级，最后求出 $a = 3.01325, b = 1.97599, c = 1.01113$ ，与 $a = 3, b = 2, c = 1$ 非常接近了。