Ceres入门——Ceres的基本使用方法
Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。Ceres官网上的文档非常详细地介绍了其具体使用方法，相比于另外一个在slam中被广泛使用的图优化库G2O，ceres的文档可谓相当丰富详细。本篇博客介绍一下Ceres库的基本使用方法。
Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：
这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。
这一部分被成为残差块（ResidualBlock），其中的 叫做代价函数（CostFunction）。代价函数依赖于一系列参数 ，这一系列参数（均为标量）称为参数块（ParameterBlock）。当然参数块中也可以只含有一个变量。 和 分别是变量块 的上下边界。
是损失函数（LossFunction） 。按照损失函数的是一个标量函数，其作用是减少异常值（Outliers）对优化结果的影响。其效果类似于对函数的过滤。
1.使用流程
使用Ceres求解非线性优化问题，可以分为三个步骤：
1. 构建代价函数（cost function）
代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符。关于仿函数的内容可以参考这篇博客：c++仿函数 functor
2.通过代价函数构建待求解的优化问题
3.配置求解器参数并求解问题
这个步骤就是设置方程怎么求解、求解过程是否输出等
2.实例分析——HelloWorld
以Ceres安装文件中example文件夹下的helloworld.cc示例程序来对Ceres库的基本使用过程进行分析。在Hello World这个例子中，待优化的函数是 。
先看源码：
#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;
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项，本问题比较简单，添加一个就行。
//第三部分： 配置并运行求解器
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
options.minimizer_progress_to_stdout = true;//输出到cout
Solver::Summary summary;//优化信息
Solve(options, &problem, &summary);//求解!!!
std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
//最终结果
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}
2.1 构建代价函数（cost function）
待优化函数为 ，也就是说我们要寻找最优的 值使 最小。所以我们的误差项为 。
struct CostFunctor {
template <typename T> bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];//误差项
return true;
}
};
2.2 构建寻优问题
首先，定义Problem类型的变量，然后将构建的代价函数添加到寻优问题中。AutoDiffCostFunction将刚刚建立的CostFunctor 结构的一个实例作为输入，自动生成其微分并且赋予其一个CostFunction 类型的接口。
Problem problem;
CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项，本问题比较简单，添加一个就行。
2.3 配置并运行求解器
为求解这个优化问题，我们需要做一些配置。这一部分很好理解，创建一个Option，配置一下求解器的配置，创建一个Summary。最后调用Solve方法，求解。
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
options.minimizer_progress_to_stdout = true;//输出到cout
Solver::Summary summary;//优化信息
Solve(options, &problem, &summary);//求解!!!
2.4 测试结果
测试结果如下，经过3次迭代之后求出x的最优解为10.
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.250000e+01 0.00e+00 5.00e+00 0.00e+00 0.00e+00 1.00e+04 0 1.91e-05 6.60e-05
1 1.249750e-07 1.25e+01 5.00e-04 5.00e+00 1.00e+00 3.00e+04 1 3.00e-05 1.31e-04
2 1.388518e-16 1.25e-07 1.67e-08 5.00e-04 1.00e+00 9.00e+04 1 7.87e-06 1.46e-04
Ceres Solver Report: Iterations: 3, Initial cost: 1.250000e+01, Final cost: 1.388518e-16, Termination: CONVERGENCE
x : 5 -> 10
3.实例分析——非线性拟合
以Ceres安装文件中另一个示例是使用Ceres来进行非线性拟合。它指定一系列的点对来拟合一个曲线的系数。这一系列点对是通过曲线 插值的点然后添加了标准差的高斯噪声。我们要拟合的曲线形式为：
先看代码：
#include <iostream>
#include "ceres/ceres.h"
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
const int kNumObservations = 67;
//观测值
const double data[] = {
0.000000e+00, 1.133898e+00,
7.500000e-02, 1.334902e+00,
1.500000e-01, 1.213546e+00,
2.250000e-01, 1.252016e+00,
3.000000e-01, 1.392265e+00,
3.750000e-01, 1.314458e+00,
4.500000e-01, 1.472541e+00,
5.250000e-01, 1.536218e+00,
6.000000e-01, 1.355679e+00,
6.750000e-01, 1.463566e+00,
7.500000e-01, 1.490201e+00,
8.250000e-01, 1.658699e+00,
9.000000e-01, 1.067574e+00,
9.750000e-01, 1.464629e+00,
1.050000e+00, 1.402653e+00,
1.125000e+00, 1.713141e+00,
1.200000e+00, 1.527021e+00,
1.275000e+00, 1.702632e+00,
1.350000e+00, 1.423899e+00,
1.425000e+00, 1.543078e+00,
1.500000e+00, 1.664015e+00,
1.575000e+00, 1.732484e+00,
1.650000e+00, 1.543296e+00,
1.725000e+00, 1.959523e+00,
1.800000e+00, 1.685132e+00,
1.875000e+00, 1.951791e+00,
1.950000e+00, 2.095346e+00,
2.025000e+00, 2.361460e+00,
2.100000e+00, 2.169119e+00,
2.175000e+00, 2.061745e+00,
2.250000e+00, 2.178641e+00,
2.325000e+00, 2.104346e+00,
2.400000e+00, 2.584470e+00,
2.475000e+00, 1.914158e+00,
2.550000e+00, 2.368375e+00,
2.625000e+00, 2.686125e+00,
2.700000e+00, 2.712395e+00,
2.775000e+00, 2.499511e+00,
2.850000e+00, 2.558897e+00,
2.925000e+00, 2.309154e+00,
3.000000e+00, 2.869503e+00,
3.075000e+00, 3.116645e+00,
3.150000e+00, 3.094907e+00,
3.225000e+00, 2.471759e+00,
3.300000e+00, 3.017131e+00,
3.375000e+00, 3.232381e+00,
3.450000e+00, 2.944596e+00,
3.525000e+00, 3.385343e+00,
3.600000e+00, 3.199826e+00,
3.675000e+00, 3.423039e+00,
3.750000e+00, 3.621552e+00,
3.825000e+00, 3.559255e+00,
3.900000e+00, 3.530713e+00,
3.975000e+00, 3.561766e+00,
4.050000e+00, 3.544574e+00,
4.125000e+00, 3.867945e+00,
4.200000e+00, 4.049776e+00,
4.275000e+00, 3.885601e+00,
4.350000e+00, 4.110505e+00,
4.425000e+00, 4.345320e+00,
4.500000e+00, 4.161241e+00,
4.575000e+00, 4.363407e+00,
4.650000e+00, 4.161576e+00,
4.725000e+00, 4.619728e+00,
4.800000e+00, 4.737410e+00,
4.875000e+00, 4.727863e+00,
4.950000e+00, 4.669206e+00,
};
//1.代价函数结构体
struct ExponentialResidual {
ExponentialResidual(double x, double y)
: x_(x), y_(y) {}
template <typename T> bool operator()(const T* const m, const T* const c, T* residual) const {
residual[0] = y_ - exp(m[0] * x_ + c[0]);
return true;
}
private:
const double x_;
const double y_;
};
int main(int argc, char** argv) {
double m = 0.0;
double c = 0.0;
//构建寻优问题
Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
problem.AddResidualBlock(
new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
new ExponentialResidual(data[2 * i], data[2 * i + 1])),
NULL,
&m, &c);
}
//配置并运行求解器
Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << "\n";
std::cout << "Initial m: " << 0.0 << " c: " << 0.0 << "\n";
std::cout << "Final m: " << m << " c: " << c << "\n";
return 0;
}
3.1 构建代价函数
这里的代价函数结构体和第二节中的代价函数结构体有些区别。在第二节中，我们待求的参数是 ，所以残差项是 。但是在此处，我们待求解的参数是 和 ，相应的残差项为 。相对来说，上一节的代价函数结构体中并没有真正意义上利用仿函数的优势，而此节中的代价函数结构体则充分体现了放函数的优势。
ExponentialResidual结构体定义了两个私有成员变量 x_ 和 y_ ，并且在构造函数中将构造函数的参数赋值给这两个变量。这样就可以在重定义 () 操作符时，仅通过输入两个参数 和 就能计算残差。
struct ExponentialResidual {
ExponentialResidual(double x, double y)
: x_(x), y_(y) {}
//重定义()操作符
template <typename T> bool operator()(const T* const m, const T* const c, T* residual) const {
residual[0] = y_ - exp(m[0] * x_ + c[0]); //残差项 res = y - exp(m[0]*x-c[0])
return true;
}
private:
const double x_;
const double y_;
};
3.2 构建寻优问题
我们要拟合的曲线是 此时我们有67组观测值。想要求得参数 和 的值，我们需要构建如下目标函数：
因此我们需要用 循环将残差项加入到我们定义的 当中。
Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
problem.AddResidualBlock(
new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
new ExponentialResidual(data[2 * i], data[2 * i + 1])),
NULL,
&m, &c);
}
3.3 配置并运行求解器
在这部分中与第二节不同的是我们设定了最大迭代次数，其他无异。
//配置并运行求解器
Solver::Options options;
options.max_num_iterations = 25;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary);
3.4 测试结果
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.211734e+02 0.00e+00 3.61e+02 0.00e+00 0.00e+00 1.00e+04 0 2.91e-04 3.44e-04
1 2.334822e+03 -2.21e+03 0.00e+00 7.52e-01 -1.87e+01 5.00e+03 1 3.31e-05 4.15e-04
2 2.331438e+03 -2.21e+03 0.00e+00 7.51e-01 -1.86e+01 1.25e+03 1 1.10e-05 4.34e-04
3 2.311313e+03 -2.19e+03 0.00e+00 7.48e-01 -1.85e+01 1.56e+02 1 1.00e-05 4.49e-04
4 2.137268e+03 -2.02e+03 0.00e+00 7.22e-01 -1.70e+01 9.77e+00 1 9.06e-06 4.62e-04
5 8.553131e+02 -7.34e+02 0.00e+00 5.78e-01 -6.32e+00 3.05e-01 1 1.41e-05 4.80e-04
6 3.306595e+01 8.81e+01 4.10e+02 3.18e-01 1.37e+00 9.16e-01 1 2.83e-04 7.67e-04
7 6.426770e+00 2.66e+01 1.81e+02 1.29e-01 1.10e+00 2.75e+00 1 2.93e-04 1.07e-03
8 3.344546e+00 3.08e+00 5.51e+01 3.05e-02 1.03e+00 8.24e+00 1 2.82e-04 1.36e-03
9 1.987485e+00 1.36e+00 2.33e+01 8.87e-02 9.94e-01 2.47e+01 1 2.81e-04 1.64e-03
10 1.211585e+00 7.76e-01 8.22e+00 1.05e-01 9.89e-01 7.42e+01 1 2.81e-04 1.93e-03
11 1.063265e+00 1.48e-01 1.44e+00 6.06e-02 9.97e-01 2.22e+02 1 2.82e-04 2.22e-03
12 1.056795e+00 6.47e-03 1.18e-01 1.47e-02 1.00e+00 6.67e+02 1 2.80e-04 2.50e-03
13 1.056751e+00 4.39e-05 3.79e-03 1.28e-03 1.00e+00 2.00e+03 1 2.83e-04 2.79e-03
Ceres Solver Report: Iterations: 14, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: CONVERGENCE
Initial m: 0 c: 0
Final m: 0.291861 c: 0.131439
可以看出，经过14次迭代之后，计算出 ， ，与真实值差异不大。
参考博客：
https://blog.csdn.net/wzheng92/article/details/79634069
https://blog.csdn.net/cqrtxwd/article/details/78956227