视觉SLAM实践入门——（8）使用Ceres库进行曲线拟合

Ceres是一个最小二乘问题求解库。Ceres求解的最小二乘问题一般形式为

x是优化变量，也称为参数块，f是代价函数，也称为残差块。l和u是某个优化变量的上限和下限

使用Ceres求解的步骤：

1、定义误差类型CURVE_FITTING_COST，其中residual是残差

2、定义待估计变量

3、定义Problem对象，创建残差块并通过AddResidualBlock添加到Problem中，其中AutoDiffCostFunction是自动求导方式

4、定义Options对象，设置配置项

5、定义Summary对象，用于存放优化信息

6、使用Solve函数求解

 

 

 

 

#include <iostream>
#include <ceres/ceres.h>
#include <opencv2/core/core.hpp>
#include <chrono>

using namespace cv;
using namespace std;

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

	template<typename T>
	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]);	//误差变量
		return true;
	}
	const double _x, _y;
};


int main(void)
{
	double ar = 1.0, br = 2.0, cr = 1.0;
	double ae = 2.0, be = -1.0, ce = 5.0;
	int N = 100;
	double sigma = 1.0;
	double inv_sigma = 1.0 / sigma;
	cv::RNG rng;

	vector<double> x_data, y_data;
	for(int i = 0; i < N; i++)
	{
		double x = 1.0 * i / N;
		x_data.push_back(x);
		y_data.push_back(exp(ar*x*x + br*x + cr) + rng.gaussian(sigma*sigma));
	}

	double abc[3] = {ae, be, ce};	//待估计参数

	ceres::Problem problem;
	for(int i = 0; i < N; i++)
	{
		problem.AddResidualBlock
		(
			//自动求导，模板参数：误差类型，输出维度（待估计参数），输入维度（误差）
			new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
			nullptr,	//核函数，没使用
			abc	//待估计参数	
		);
	}

	ceres::Solver::Options opt;	//配置项
	opt.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;	//利用cholesky分解，求解增量方程
	opt.minimizer_progress_to_stdout = true;	//优化过程输出到cout

	ceres::Solver::Summary summary;	//优化信息
	chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
	ceres::Solve(opt, &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 << "time_used = " << time_used.count() << "s" << endl;

	cout << summary.BriefReport() << endl;
	cout << "estimated a,b,c = ";
	for(auto a:abc)	cout << a << " ";
	cout << endl;


	return 0;
}