slam book 14 ch6 CERES

记录 slam book 14 ch6 CERES

使用自动求导和手动求导

关于其雅克比可以参考书籍

// 采用非自动求导的形式进行优化

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

using namespace std;

// 数据产生模块
void product(vector<double>& x_data, vector<double>& y_data, int N = 100, double ar = 1.0,double br = 2.0,double cr = 1.0){

	double w_sigma = 1.0;                        // 噪声Sigma值
	double inv_sigma = 1.0 / w_sigma;
	cv::RNG rng;                                 // OpenCV随机数产生器

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

// 代价函数的计算模型
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数据
};


void ceresAuto(){
	
	vector<double> x_data;
	vector<double> y_data;
	int N = 100;
	product(x_data, y_data);
	double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
	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                 // 待估计参数cmmmmmmmmmmmmmmmmmmmmm
		);
	}
	// 配置求解器
	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;
}

// 非自动求导进行计算
// 残差1维度 输入： 3
class ErrorCost : public ceres::SizedCostFunction<1, 3> {
public:
    ErrorCost(double y, double x):y_(y), x_(x){
    }
    // 计算jacobin等
    bool Evaluate(const double *const *parameters, double *residuals, double **jacobians) const override {

		double a = parameters[0][0];
		double b = parameters[0][1];
		double c = parameters[0][2];
		// 求误差
		residuals[0] = y_ - ceres::exp(a * x_ * x_ + b * x_ + c);
		// 求jacobin 是 1 × 3的
		double expc = ceres::exp(a * x_ * x_ + b * x_ + c);
		if (jacobians != NULL && jacobians[0] != NULL)
		{
			jacobians[0][0] = - x_ * x_ * expc;
			jacobians[0][1] = - x_ * expc;
			jacobians[0][2] = - expc;
		}
		return true;
    }

private:
	double y_;
	double x_;
};

void ceresNoAuto(){
	
	vector<double> x_data;
	vector<double> y_data;
	int N = 100;
	product(x_data, y_data);
	double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
	double abc[3] = {ae, be, ce};

	// 构建最小二乘问题
	ceres::Problem problem;

	problem.AddParameterBlock(abc,3);

	for (int i = 0; i < N; i++) {
		problem.AddResidualBlock(
			new ErrorCost(y_data[i], x_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;
}

int main(int argc, char **argv) {
	
	//ceresAuto();
	ceresNoAuto();
	return 0;
}

cmakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(ch6)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

# Ceres
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})

# sophus 
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# Eigen
include_directories("/usr/include/eigen3")
find_package(gflags REQUIRED)
add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS}  ${CERES_LIBRARIES})