Ceres Solver

Posted on In Waline:

Ceres-Solver 库的调用方式整理。

Ceres Solver Tutorials

1　介绍

Ceres Solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达 SLAM 项目 cartographer 中被大量使用。相比于 g2o，Ceres 的文档丰富详细。

2　使用 Ceres 求解非线性优化问题

Step. 1　构建损失函数，具体是定义 cost function 结构体，在内部重载 () 运算符；
Step. 2　通过损失函数构建优化问题；
Step. 3　配置求解器参数并求解问题，即设置方程怎么求解、求解过程是否输出，然后调用 solve 方法。

3　举例

参考 Ceres 官网，求解问题

x∗=arg⁡min⁡x12(10−x)2x^* = \arg \min_x \frac{1}{2} (10 - x)^2 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include <iostream>
#include <ceres/ceres.h>

using namespace std;
using namespace ceres;

// 第一部分：构建代价函数，重载 () 符号
struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {
       residual[0] = T(10.0) - x[0];
       return true;
   }
};

// 主函数
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

    // 寻优参数x的初始值，为5
    double initial_x = 5.0;
    double x = initial_x;

    // 第二部分：构建寻优问题
    Problem problem;
    // 使用自动求导，将之前的代价函数结构体传入，第一个 1 是输出维度，即残差的维度，第二个 1 是输入维度，即待寻优参数 x 的维度
    CostFunction* cost_function = 
        new AutoDiffCostFunction<CostFunctor, 1, 1>(
        	new CostFunctor
    ); 
    // 向问题中添加误差项
    problem.AddResidualBlock(cost_function, NULL, &x); 

    // 第三部分： 配置并运行求解器
    Solver::Options options;
    // 配置增量方程的解法
    options.linear_solver_type = ceres::DENSE_QR; 
    // 输出到 cout
    options.minimizer_progress_to_stdout = true;
    Solver::Summary summary;             // 优化信息
    Solve(options, &problem, &summary);  // 求解

    // 输出优化的简要信息
    std::cout << summary.BriefReport() << "\n";
	// 最终结果
    std::cout << "x : " << initial_x << " -> " << x << "\n";
    return 0;
}

4　简单进阶

4.1　其他求导法

除自动求导（AutoDiffCostFunction）外，Ceres 还有其他求导法，例如数值求导法：

1
2
3
4
5
CostFunction* cost_function = 
    new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1> (
      new NumericDiffCostFunctor
);
problem.AddResidualBlock(cost_function, NULL, &x);

4.2　曲线拟合

拟合非线性函数的曲线：

y=e3x2+2x+1y = e^{3x^2 + 2x + 1}

整个代码的思路是先构建代价函数结构体，然后在 [0,1][0,1] 之间均匀生成待拟合曲线的 1000 个数据点，加上方差为 1 的白噪声，数据点用两个 vector 储存（x_data 和 y_data），然后构建待求解优化问题，最后求解，拟合曲线参数。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>

using namespace std;
using namespace cv;

// 构建代价函数结构体，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 {
        residual[0]=_y-ceres::exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        return true;
    }
    const double _x, _y;
};

int main() {
    // 参数初始化设置，abc 初始化为0，白噪声方差为 1
    double a = 3, b = 2, c = 1;
    double w = 1;
    cv::RNG rng;
    double abc[3] = {0, 0, 0};

    // 生成待拟合曲线的数据散点，储存在 Vector 里
    std::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(exp(a * x * x + b * x + c ) + rng.gaussian(w));
  	}

	// 反复使用 AddResidualBlock 方法
	// 将每个点的残差累计求和构建最小二乘优化式
	// 不使用核函数，待优化参数是 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
    	);
  	}

	// 配置求解器并求解，输出结果
  	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;
	return 0;
}

4.3　鲁棒曲线拟合

求解优化问题中（比如拟合曲线），数据中往往会有离群点、错误值什么的，最终得到的寻优结果很容易受到影响，此时就可以使用一些损失核函数来对离群点的影响加以消除。要使用核函数，只需要把上述代码中添加残差块函数中的 NULL 或 nullptr 换成损失核函数结构体的实例。

Ceres库中提供的核函数主要有：TrivialLoss 、HuberLoss、 SoftLOneLoss 、 CauchyLoss。比如此时要使用 CauchyLoss，只需要将 nullptr 换成 new CauchyLoss(0.5) 就行。

参考