这里写目录标题
- 说明
- 步骤
- 源代码
说明
Google Ceres 是一个广泛使用的最小二乘问题求解库。在 Ceres 中,只需按照一定步骤定义待解的优化问题,然后交给求解器计算即可。
步骤
- 定义每个参数块。
参数块通常为平凡的向量,但是在 SLAM 里也可以定义成四元数、李代数这种特殊的结构。如果是向量,那么需要为每个参数块分配一个 double 数组,来存储变量的值。 - 定义残差块的计算方式。
残差块通常关联若干个参数块,对它们进行一些自定义的计算,然后返回残差值。Ceres 对它们求平方和之后,作为目标函数的值。 - 残差块往往也需要定义雅可比的计算方式。
在 Ceres 中,你可以使用它提供的“自动求导”功能,也可以手动指定雅可比的计算过程。如果要使用自动求导,那么残差块需要按照特定的写法来书写:残差的计算过程应该是一个带模板的括号运算符。 - 把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中,调用 Solve 函数求解即可。
求解之前,可以传入一些配置信息,例如迭代次数、终止条件等,也可以使用默认的配置。
源代码
#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; // 输出到coutceres::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;
}
- 定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的 () 运算符,这样该类就成为了一个拟函数(Functor)。这种定义方式使得 Ceres 可以像调用函数一样,对该类的某个对象(比如 a)调用 a() 方法。事实上,Ceres 会把雅可比矩阵作为类型参数传入此函数,从而实现自动求导的功能。
- 程序中的 double abc[3] 即为参数块,而对于残差块,我们对每一个数据构造 CURVE_FITTING_COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,我们有若干种选择:(1)使用 Ceres 的自动求导(Auto Diff);(2)使用数值求导(Numeric Diff);(3)自行推导解析的导数形式,提供给 Ceres。因为自动求导在编码上是最方便的,于是我们使用自动求导。
- 自动求导需要指定误差项和优化变量的维度。这里的误差是标量,维度为 1;优化的是 a, b, c三个量,维度为 3。于是,在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
- 设定好问题后,调用 Solve 函数进行求解。你可以在 options 里配置(非常详细的)优化选项。例如,可以选择使用 Line Search 还是 Trust Region、迭代次数、步长,等等。