by jie 2018.6.22
ceres 简介
Ceres Solver是谷歌2010就开始用于解决优化问题的C++库，2014年开源．在Google地图，Tango项目，以及著名的SLAM系统OKVIS和Cartographer的优化模块中均使用了Ceres Solver.
在SLAM领域优化问题还可以使用g2o来求解．不过Ceres提供了自动求导功能，虽然是数值求导，但可以避免复杂的雅克比计算，目前来看Ceres相对于g2o的缺点仅仅是依赖的库多一些（g2o仅依赖Eigen）．但是提供了可以直接对数据进行操作的能力，相对于g2o应用在视觉SLAM中，更加倾向于通用的数值优化，最重要的是提供的官方资料比较全（看g2o简直受罪．．．）．详细的介绍可以参考google的文档：http://ceres-solver.org/features.html
重要概念：
- ObjectiveFunction：目标函数；
- ResidualBlock：残差（实际观察值与估计值之间的差），多个ResidualBlock组成完整的目标函数；
- CostFunction：代价函数，观测数据与估计值的差，观测数据就是传感器获取的数据，估计值是使用别的方法获取（例如初始化，ICP，PnP或者匀速模型．．．）的从优化变量通过建模得出的观测值；例如从对极几何得到的相机位姿，三角化得到的地图点可以作为优化变量的初始值，但是需要利用坐标系变换和相机模型转化为2D平面上的像素坐标估计值，与实际测量得到的观测值之间构建最小二乘问题；
- ParameterBlock：优化变量；
- LossFunction：核函数，用来减小Outlier的影响，对应g2o中的edge->setRobustKernel()
- Ceres can also use Eigen as a sparse linear algebra library.
- SuiteSparse. Needed for solving large sparse linear systems. Optional; strongly recomended for large scale bundle adjustment
- CXSparse. Similar to SuiteSparse but simpler and slower. CXSparse has no dependencies on LAPACK and BLAS. This makes for a simpler build process and a smaller binary. Optional
参考 Ceres优化
优化方法
- Trust Region Solvers - Ceres supports Levenberg-Marquardt, Powell’s Dogleg, and Subspace dogleg methods. The key computational cost in all of these methods is the solution of a linear system. To this end Ceres ships with a variety of linear solvers - dense QR and dense Cholesky factorization (using Eigen or LAPACK) for dense problems, sparse Cholesky factorization (SuiteSparse, CXSparse or Eigen) for large sparse problems custom Schur complement based dense, sparse, and iterative linear solvers for bundle adjustment problems.
- Line Search Solvers - When the problem size is so large that storing and factoring the Jacobian is not feasible or a low accuracy solution is required cheaply, Ceres offers a number of line search based algorithms. This includes a number of variants of Non-linear Conjugate Gradients, BFGS and LBFGS.
安装方法
参考：在Ubuntu16.04下安装Ceres
1、Ceres是一个cmak工程，首先要安装他的依赖项，使用apt-get安装。
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev
libgoogle-glog-dev libgtest-dev
2、如果安装时找不到 cxsparse 或者其他的lib，需要添加下面的源。
（1）先使用如下命令打开source.list
sudo gedit /etc/apt/sources.list
（2）然后讲下面的源粘贴到source.list的最上方
deb http://cz.archive.ubuntu.com/ubuntu trusty main universe
（3）更新源
sudo apt-get update
（4）在输入1中的命令安装依赖项。
sudo make install
3、Ceres库是来自谷歌的非线性优化库，建议去github上下载，也可以在我的博客里下载。下载并解压后，切换到Ceres库所在目录，按如下步骤输入命令编译和安装安装。
mkdir build
cd build
cmake ..
make
这个过程会花费几分钟，如果你的电脑支持GPU加速，也可以用make -j4 来开启4个线程加速编译。
4、编译完成后安装。
sudo make install
5、Ceres库的头文件安装在"/usr/local/include/ceres/"目录下，库文件安装在"/usr/local/lib/"目录下。安装完成后查看是否有对应的文件，如果有则说明安装成功。
使用流程
参考 一文助你Ceres 入门——Ceres Solver新手向全攻略
使用Ceres求解非线性优化问题，一共分为三个部分：
1、 第一部分：构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符，具体实现方法后续介绍。
2、 第二部分：通过代价函数构建待求解的优化问题。
3、 第三部分：配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。
好了，此时你应该对ceres的大概使用流程有了一个基本的认识。下面我就基于ceres官网上的教程中的一个例程来详细介绍一下ceres的用法。
Ceres官网教程给出的例程中，求解的问题是求x使得12(10−x)2
取到最小值。（很容易心算出x的解应该是10）
好，来看代码：
#include
#include
using namespace std;
using namespace ceres;
//第一部分：构建代价函数，重载（）符号，仿函数的小技巧
struct CostFunctor {
template
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(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;
}
第一部分：构造代价函数结构体
这里值得注意的是，必须要编写一个重载()运算，返回值为bool， 而且必须使用模板类型，所有的输入参数和输出参数都要使用T类型，
参数的名字和类型可以自己定义。好像不能有多个模板类型，比如 template
这里的使用了仿函数的技巧，即在CostFunction结构体内，对（）进行重载，这样的话，该结构体的一个实例就能具有类似一个函数的性质，在代码编写过程中就能当做一个函数一样来使用。
关于仿函数，这里再多说几句，对结构体、类的一个实例，比如Myclass类的一个实例Obj1，如果Myclass里对（）进行了重载，那Obj1被创建之后，就可以将Obj1这个实例当做函数来用，比如Obj（x）这样，为了方便读者理解，下面随便编一段简单的示例代码，凑活看看吧。
//仿函数的示例代码
#include
using namespace std;
class Myclass
{
public:
Myclass(int x):_x(x){};
int operator()(const int n)const{
return n*_x;
}
private:
int _x;
};
int main()
{
Myclass Obj1(5);
cout< 在我随便写的示教代码中，可以看到我将Myclass的（）符号的功能定义成了将括号内的数n乘以隐藏参数x倍，其中x是Obj1对象的一个私有成员变量，是是在构造Obj1时候赋予的。因为重载了（）符号，所以在主函数中Obj1这个对象就可以当做一个函数来使用，使用方法为Obj1（n），如果Obj1的内部成员变量_x是5，则此函数功能就是将输入参数扩大5倍，如果这个成员变量是50，Obj1（）函数的功能就是将输入n扩大50倍，这也是仿函数技巧的一个优点，它能利用对象的成员变量来储存更多的函数内部参数。
了解了仿函数技巧的使用方法后，再回过头来看看ceres使用中构造CostFuction 的具体方法：
CostFunction结构体中，对括号符号重载的函数中，传入参数有两个，一个是待优化的变量x，另一个是残差residual，也就是代价函数的输出。
重载了（）符号之后，CostFunction就可以传入AutoDiffCostFunction方法来构建寻优问题了。
第二部分：通过代价函数构建待求解的优化问题
Problem problem;
CostFunction* cost_function =
new AutoDiffCostFunction(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
这一部分就是待求解的优化问题的构建过程，使用之前结构体创建一个实例，由于使用了仿函数技巧，该实例在使用上可以当做一个函数。基于该实例new了一个CostFunction结构体，这里使用的自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。分别对应之前结构体中的residual和x。
向问题中添加误差项，本问题比较简单，添加一次就行（有的问题要不断多次添加ResidualBlock以构建最小二乘求解问题）。这里的参数NULL是指不使用核函数，&x表示x是待寻优参数。
第三部分：配置问题并求解问题
Solver::Options options;
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 << "x : " << initial_x
<< " -> " << x << "\n";
这一部分很好理解，创建一个Option，配置一下求解器的配置，创建一个Summary。最后调用Solve方法，求解。
最后输出结果：
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 4.512500e+01 0.00e+00 9.50e+00 0.00e+00 0.00e+00 1.00e+04 0 5.33e-04 3.46e-03
1 4.511598e-07 4.51e+01 9.50e-04 9.50e+00 1.00e+00 3.00e+04 1 5.00e-04 4.05e-03
2 5.012552e-16 4.51e-07 3.17e-08 9.50e-04 1.00e+00 9.00e+04 1 1.60e-05 4.09e-03
Ceres Solver Report: Iterations: 2, Initial cost: 4.512500e+01, Final cost: 5.012552e-16, Termination: CONVERGENCE
x : 0.5 -> 10
读者们看到这里相信已经对Ceres库的使用已经有了一个大概的认识，现在可以试着将代码实际运行一下来感受一下，加深一下理解。
博主的使用环境为Ubuntu 16.04，所以在此附上CMakeList.txt，怎么样，是不是很贴心：）。
附：CMakeLists.txt代码：
//CMakeLists.txt：
cmake_minimum_required(VERSION 2.8)
project(ceres)
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
add_executable(use_ceres use_ceres.cpp)
target_link_libraries(use_ceres ${CERES_LIBRARIES})
进阶-更多的求导法
在上面的例子中，使用的是自动求导法（AutoDiffCostFunction），Ceres库中其实还有更多的求导方法可供选择（虽然自动求导的确是最省心的，而且一般情况下也是最快的。。。）。这里就简要介绍一下其他的求导方法：
数值求导法（一般比自动求导法收敛更慢，且更容易出现数值错误）：
数值求导法的代价函数结构体构建和自动求导中的没有区别，只是在第二部分的构建求解问题中稍有区别，下面是官网给出的数值求导法的问题构建部分代码:
CostFunction* cost_function =
new NumericDiffCostFunction(
new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
乍一看和自动求导法中的代码没区别，除了代价函数结构体的名字定义得稍有不同，使用的是NumericDiffCostFunction而非AutoDiffCostFunction，改动的地方只有在模板参数设置输入输出维度前面加了一个模板参数ceres::CENTRAL，表明使用的是数值求导法。
还有其他一些更多更复杂的求导法，不详述。
再进阶-曲线拟合
趁热打铁，阅读到这里想必读者们应该已经对Ceres库的使用已经比较了解了（如果前面认真看了的话），现在就来尝试解决一个更加复杂的问题来检验一下成果，顺便进阶一下。
问题：
拟合非线性函数的曲线（和官网上的例子不一样，稍微复杂一丢丢）：
y=e3x2+2x+1
依然，先上代码：
代码之前先啰嗦几句，整个代码的思路还是先构建代价函数结构体，然后在[0,1]之间均匀生成待拟合曲线的1000个数据点，加上方差为1的白噪声，数据点用两个vector储存（x_data和y_data），然后构建待求解优化问题，最后求解，拟合曲线参数。
（PS. 本段代码中使用OpenCV的随机数产生器，要跑代码的同学可能要先装一下OpenCV）
#include
#include
#include
using namespace std;
using namespace cv;
//构建代价函数结构体，abc为待优化参数，residual为残差。
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
template
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（使用OpenCV的随机数产生器）。
double a=3,b=2,c=1;
double w=1;
RNG rng;
double abc[3]={0,0,0};
//生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
vector 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方法（逐个散点，反复1000次）
//将每个点的残差累计求和构建最小二乘优化式
//不使用核函数，待优化参数是abc
ceres::Problem problem;
for(int i=0;i<1000;i++)
{
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction(
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= "<
void FscanfOrDie(FILE *fptr, const char *format, T *value) {
int num_scanned = fscanf(fptr, format, value);
if (num_scanned != 1) {
LOG(FATAL) << "Invalid UW data file.";
}
}
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
int* point_index_;
int* camera_index_;
double* observations_;
double* parameters_;
};
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - observed_y;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y) {
return (new ceres::AutoDiffCostFunction(
new SnavelyReprojectionError(observed_x, observed_y)));
}
double observed_x;
double observed_y;
};
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
if (argc != 2) {
std::cerr << "usage: simple_bundle_adjuster \n";
return 1;
}
BALProblem bal_problem;
if (!bal_problem.LoadFile(argv[1])) {
std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
return 1;
}
const double* observations = bal_problem.observations();
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(observations[2 * i + 0],
observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
}
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
return 0;
}
程序读入BA数据集，因此先了解下数据集。
数据集
Bundle Adjustment in the Large
University of Washington' Bundle Adjustment in the Large dataset
该数据集用在ceres和g2o练习中求解BA问题，其定义了各个相机位姿和路标点的关系。
数据格式
Data Format
Each problem is provided as a bzip2 compressed text file in the following format.
...
...
...
Where, there camera and point indices start from 0. Each camera is a set of 9 parameters - R,t,f,k1 and k2. The rotation R is specified as a Rodrigues' vector.
- 也就是说第一行定义了相机数量（帧数量）m，路标数量（特征点数量）n，和总的观测量k。
- 第2行开始，一直到第k+1行，定义了第i帧观测到了第j个特征点所看到的像素坐标（x,y).
- 接下来m×9行定义了每帧相机的位姿，R,t,f,k1 and k2。这里R是3维Rodrigues' vector，也就是罗德里格斯旋转向量，可以通过罗德里格斯公式求得旋转向量，见slam book p71。t是3维平移向量，f是焦距，k1和k2是径向畸变
- 接下来n×3行是n个特征点的三维坐标
代码分析
- 构造：
cost_function。
传入txt文件读入的 “观测到的像素坐标系下的二维特征点（x，y）”
ceres::CostFunction* cost_function =
SnavelyReprojectionError::Create(observations[2 * i + 0],
observations[2 * i + 1]);
- 添加误差项。
传入txt文件读入的 “相机位姿（R,t,f,k1 and k2），3维特征点”。对比cere拟合demo程序，传入的相机位姿：mutable_camera_for_observation(i)和3维特征点：mutable_point_for_observation(i)是要求的待优化的变量。
problem.AddResidualBlock(cost_function,
NULL /* squared loss */,
bal_problem.mutable_camera_for_observation(i),
bal_problem.mutable_point_for_observation(i));
过程为
mutable_camera_for_observation(i)
-> mutable_cameras() + camera_index_[i] * 9;
这里mutable_cameras()是存储的cam的首地址,也就是返回第i帧的相机位姿的9个变量。
3.求残差。
// 计算畸变的中心，符号依赖于Noah Snavely的假设
// 相机有一个负的z轴
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
// 应用二阶和四阶径向畸变
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// 计算最终的投影点位置
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// The error is the difference between the predicted and observed position.
// 误差是预测值和观测值的区别
residuals[0] = predicted_x - T(observed_x);
residuals[1] = predicted_y - T(observed_y);
畸变投影模型见slam book p246
这里没有考虑cx，cy,应该是给出的observed_x和observed_y是相对于光心的坐标，因此不需要将算出的坐标系平移到左上角。
一些细节：
AutoDiff , NumericDiff, AnalyticDiff 的 bool operator() const {} , bool operator() const {}, virtual bool Evaluate() const 都必须是const 成员函数；
AutoDiff 的 bool operator() const {}的函数体中， 当数值要和 T 类型变量，相加减，乘除时， 要先转换为T类型，再运算。
AutoDiff 的 bool operator() const {}是模板函数对象， 假如在函数体内部要调用另一个模板函数 B， 发现当这个模板函数B在自己本结构体中定义时，调用的时候，会报 ”this **** 不能转换为 **** 的引用”。解决办法： 把模板函数B在结构体外定义，再调用就可以了。
AutoDiffCostFunction:
CostFunction* cost_function= new AutoDiffCostFunction(
new MyScalarCostFunctor(1.0));
it means
Dimension of residual,
Dimension of x ,
Dimension of y
表示式中ρi(‖fi(xi1,⋯,xik)‖2)被称为残差块ResidualBlock，其中f(⋅)是依赖于参数块[xi1,⋯,xik]的一个代价函数。lj和uj式参数xj的边界。ρ(⋅)式损耗函数，用于减少离群值outliers对解的影响。
参考资料
ceres_solver
Ceres-Solver学习笔记(1-8) 重点推荐，系列文章
Ceres Solver使用