SLAM知识点——Ceres库学习

Ceres solver 是谷歌开发的一款用于非线性优化的C++库，最小二乘求解器，只需要函数和变量定义出来，求解器（算法）选好，就可以ceres去解。有自动求导的功能。优化算法在视觉里程计和后端中都
会出现。

下面内容引用高翔《视觉SLAM十四讲》:
"Ceres 库面向通用的最小二乘问题的求解，作为用户，我们需要做的就是定义优化问题，然后设置一些选项，输入进 Ceres 求解即可。Ceres 求解的最小二乘问题最一般的形式如下（带边界的核函数最小二乘）：
$$\begin{array}{l}\underset{x}{\min }\frac{1}{2}\sum _i{\rho }_i\left({\Vert f_i\left(x_{i1},...,x_{in}\right)\Vert }^2\right)\\ \mathrm{s}.t.{\mathrm{l}}_j\le x_j\le u_j\end{array}$$可以看到，目标函数由许多平方项，经过一个核函数${\rho }_i(\cdot )$之后，求和组成。在最简单的情况下，取${\rho }_i$为恒等函数，则目标函数即为许多项的平方和。在这个问题中，优化变量为$x_{i1},...,x_{in}$，$f_i$ 称为代价函数（Cost function），在 SLAM 中亦可理解为误差项。lj 和 uj 为第 j 个优化变量的上限和下限。在最简单的情况下，取${\mathrm{l}}_j=\infty ,u_j=\infty$（不限制优化变量的边界），并且取${\rho }_i$为恒等函数时，就得到了无约束的最小二乘问题。
在 Ceres 中，我们将定义优化变量 x 和每个代价函数$f_i$ ，再调用 Ceres 进行求解。我们可以选择使用 G-N 或者 L-M 进行梯度下降，并设定梯度下降的条件，Ceres 会在优化之后，将最优估计值返回给我们。”

1 安装

下载：https://download.csdn.net/download/qq_46515446/45021265
解压:
tar -xzvf ceres-solver.tar.gz
cd ceres-solver
mkdir build
cd build
cmake …
注意此时会报错，会提醒缺少依赖项， Failed to find CXSparse
sudo apt-get install libcxsparse3.1.4
sudo apt-get install libgoogle-glog-dev libsuitesparse-dev libgtest-dev
cmake …
make （make -j2，参考）
sudo make install

装到自己的电脑上，该功能有点类似将build下面的库和头文件安装到/usr/local/lilb和/usr/local/include/ceres下面。ceres只有一个静态库文件libceres.a

ls /usr/local/lib/ 使用该指令查看某一个目录下的文件

结束。

补充：

linux系统中lib，lib-dev库的区别：

• dev后缀（develope）：包含了库的接口（.h文件即头文件），这个为了当你开发一个程序时想要链接到这个包时。

参考：https://blog.csdn.net/pk0127/article/details/117201060

直接百度Ceres solver，可以查看官网说明文档。
Ceres solver文件夹下面有个example，里面有不少东西，曲线拟合、bundle adjustment。

2 Ceres求解最小值

cmake_module见补充：n.1 cmake_module；
仿函数见补充：n.2 仿函数；
以下内容参考：https://www.jianshu.com/p/e5b03cf22c80

示例1： Ceres官网教程给出的例程中，求解的问题是求x使得
$$\frac{1}{2}{\left(10-x\right)}^2$$取到最小值。这里已经是最小二乘的形式，$f=10-x$可以直观的看到x的最小值为10

“使用Ceres求解非线性优化问题，一共分为三个部分：
1、 第一部分：构建cost fuction，即代价函数，也就是寻优的目标式。这个部分需要使用仿函数（functor）这一技巧来实现，做法是定义一个cost function的结构体，在结构体内重载（）运算符，具体实现方法后续介绍。
2、 第二部分：通过代价函数构建待求解的优化问题。
3、 第三部分：配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等”

第二部分扩充：
“使用之前结构体创建一个实例，由于使用了仿函数技巧，该实例在使用上可以当做一个函数。基于该实例new了一个CostFunction结构体，这里使用的自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。分别对应之前结构体中的residual和x。
向问题中添加误差项，本问题比较简单，添加一次就行（有的问题要不断多次添加ResidualBlock以构建最小二乘求解问题）。这里的参数NULL是指不使用核函数，&x表示x是待寻优参数。”
使用的ceres::AutoDiffCostFunction为自动求导法，Ceres库中其实还有更多的求导方法可供选择，自动求导是最省心。

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(ceres_demo)
find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})
add_executable(ceres_demo ceres_demo.cpp)
target_link_libraries(ceres_demo ${CERES_LIBRARIES})

demo.cpp:

#include
#include
using namespace std;
//using namespace ceres;


//----第1部分----: 构建代价函数，重载（）符号
struct CostFunctor{
	template <typename T>
	bool operator()(const T* const x, T*residual) const{  //???
		residual[0] = T(10.0)-x[0];
		// residual[0] = 0.5 * pow((T(10.0)-x[0]), 2);  // 这样写是对的，0.5变成1/2就不对。
		return true;
	}
};


int main(int argc, char** argv)
{
	// 寻优参数x的初始值
	double initial_x = -3.0;
	double x = initial_x;
    //----第2部分----: 构建寻优问题
	ceres::Problem problem;
	ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。 
	problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项，本问题比较简单，添加一个就行。
	//----第3部分----: 配置并运行求解器
    ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
	options.minimizer_progress_to_stdout=true;
    //输出到cout, 输出这些信息:iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time

	ceres::Solver::Summary summary;//优化信息
	ceres::Solve(options, &problem, &summary); //求解!!!,求解完成后，输入x会更新
	cout<<"out: "<<summary.BriefReport()<<endl;//输出优化的简要信息
	cout<<"x: "<<initial_x<<" -> "<<x<<endl;
	return 0;
}

输出的结果：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  8.450000e+01    0.00e+00    1.30e+01   0.00e+00   0.00e+00  1.00e+04        0    1.71e-05    4.13e-05
   1  8.448310e-07    8.45e+01    1.30e-03   1.30e+01   1.00e+00  3.00e+04        1    3.55e-05    1.04e-04
   2  9.386386e-16    8.45e-07    4.33e-08   1.30e-03   1.00e+00  9.00e+04        1    6.36e-06    1.16e-04
out: Ceres Solver Report: Iterations: 3, Initial cost: 8.450000e+01, Final cost: 9.386386e-16, Termination: CONVERGENCE
x: -3 -> 10

cost就是代价的变化情况；iter_time当前一代迭代的时间；total_time时间是累加的；total_timeTermination: CONVERGENCE（收敛）

3 Ceres拟合曲线

示例2： 有了上面的基础，现在用Ceres来拟合非线性曲线，进阶一下。
$$y=e^{3x^2+2x+1}$$
求解整体思路：
1、构建代价函数结构体
2、在[0,1]之间均匀生成待拟合曲线的1000个数据点，加上高斯噪声（使用OpenCV的随机数产生器函数cv::RNG），数据点用两个vector储存（x_data和y_data）
3、然后构建待求解优化问题
4、求解，拟合曲线参数。

CMakeLists.txt:

cmake_minimum_required(VERSION 2.8)
project(fit_demo)

# opencv
find_package(OpenCV REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS}) # 下面这种写法也可以
#include_directories(${OpenCV_DIRS})

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

add_executable(fit_demo fit_demo.cpp)
target_link_libraries(fit_demo ${OpenCV_LIBS} ${CERES_LIBRARIES})

curve_fitting_demo.cpp:

#include
#include
#include
using namespace std;
using namespace cv;
//using namespace ceres;


//----第1部分----: 构建代价函数结构体，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] = T(y_) -ceres::exp(abc[0]*T(x_)*T(x_)+abc[1]*T(x_)+abc[2]);  //ceres::exp and exp diff 结果相同
		return true;
	}
	const double x_,y_;
};


int main(int argc, char** argv)
{
    //参数初始化设置，abc初始化为0，gaussian噪声方差为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<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));
	}

    //----第2部分----: 构建寻优问题
	//反复使用AddResidualBlock方法（逐个散点，反复1000次）
	//将每个点的残差累计求和构建最小二乘优化式
	//不使用核函数，待优化参数是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])), NULL, abc);
	}

	//----第3部分----: 配置并运行求解器
	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;
}

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  5.277388e+06    0.00e+00    5.58e+04   0.00e+00   0.00e+00  1.00e+04        0    4.68e-03    4.74e-03
   1  4.287886e+238   -4.29e+238    0.00e+00   7.39e+02  -8.79e+231  5.00e+03        1    1.16e-04    4.94e-03
   2  1.094203e+238   -1.09e+238    0.00e+00   7.32e+02  -2.24e+231  1.25e+03        1    8.34e-05    5.05e-03
   3  5.129910e+234   -5.13e+234    0.00e+00   6.96e+02  -1.05e+228  1.56e+02        1    8.26e-05    5.16e-03
   4  1.420558e+215   -1.42e+215    0.00e+00   4.91e+02  -2.97e+208  9.77e+00        1    9.19e-05    5.28e-03
   5  9.607928e+166   -9.61e+166    0.00e+00   1.85e+02  -2.23e+160  3.05e-01        1    1.03e-04    5.45e-03
   6  7.192680e+60   -7.19e+60    0.00e+00   4.59e+01  -2.94e+54  4.77e-03        1    8.24e-05    5.58e-03
   7  5.061060e+06    2.16e+05    2.68e+05   1.21e+00   2.52e+00  1.43e-02        1    6.11e-03    1.17e-02
   8  4.342234e+06    7.19e+05    9.34e+05   8.84e-01   2.08e+00  4.29e-02        1    7.14e-03    1.89e-02
   9  2.876001e+06    1.47e+06    2.06e+06   6.42e-01   1.66e+00  1.29e-01        1    4.58e-03    2.35e-02
  10  1.018645e+06    1.86e+06    2.58e+06   4.76e-01   1.38e+00  3.86e-01        1    4.50e-03    2.81e-02
  11  1.357731e+05    8.83e+05    1.30e+06   2.56e-01   1.13e+00  1.16e+00        1    9.23e-03    3.74e-02
  12  2.142986e+04    1.14e+05    2.71e+05   8.60e-02   1.03e+00  3.48e+00        1    4.82e-03    4.23e-02
  13  1.636436e+04    5.07e+03    5.94e+04   3.01e-02   1.01e+00  1.04e+01        1    4.54e-03    4.69e-02
  14  1.270381e+04    3.66e+03    3.96e+04   6.21e-02   9.96e-01  3.13e+01        1    5.99e-03    5.29e-02
  15  6.723500e+03    5.98e+03    2.68e+04   1.30e-01   9.89e-01  9.39e+01        1    4.70e-03    5.77e-02
  16  1.900795e+03    4.82e+03    1.24e+04   1.76e-01   9.90e-01  2.82e+02        1    4.51e-03    6.23e-02
  17  5.933860e+02    1.31e+03    3.45e+03   1.23e-01   9.96e-01  8.45e+02        1    8.24e-03    7.06e-02
  18  5.089437e+02    8.44e+01    3.46e+02   3.77e-02   1.00e+00  2.53e+03        1    4.75e-03    7.54e-02
  19  5.071157e+02    1.83e+00    4.47e+01   1.63e-02   1.00e+00  7.60e+03        1    5.54e-03    8.10e-02
  20  5.056467e+02    1.47e+00    3.03e+01   3.13e-02   1.00e+00  2.28e+04        1    4.58e-03    8.57e-02
  21  5.046313e+02    1.02e+00    1.23e+01   3.82e-02   1.00e+00  6.84e+04        1    5.15e-03    9.09e-02
  22  5.044403e+02    1.91e-01    2.23e+00   2.11e-02   9.99e-01  2.05e+05        1    4.61e-03    9.56e-02
  23  5.044338e+02    6.48e-03    1.38e-01   4.35e-03   9.98e-01  6.16e+05        1    8.22e-03    1.04e-01
a = 3.01325
b = 1.97599
c = 1.01113

可以看到，最终的拟合结果与真实值非常接近。
“求解优化问题中（比如拟合曲线），数据中往往会有离群点、错误值什么的，最终得到的寻优结果很容易受到影响，此时就可以使用一些损失核函数来对离群点的影响加以消除。要使用核函数，只需要把上述代码中的NULL或nullptr换成损失核函数结构体的实例。
Ceres库中提供的核函数主要有：TrivialLoss 、HuberLoss、 SoftLOneLoss 、 CauchyLoss。
比如此时要使用CauchyLoss，只需要将nullptr换成new CauchyLoss(0.5)就行（0.5为参数）。”
此时输出结果为：

a = 3.01506
b = 1.9785
c = 1.00765

可以看到结果更接近真实值。

n 补充

n.1 cmake_module

CMakeLists.txt中下面这句话的理解可以参考我的另一篇博客：https://blog.csdn.net/qq_46515446/article/details/121498619

list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

补充：opencv高斯随机数生成函数

cv::RNG A;  //opencv自带随机数生成函数
A.gaussian(1);

n.2 仿函数

仿函数，并不是函数，却有着类似于函数的行为，简单地说就是重载括号运算符号。
把对象当作一个函数用
仿函数拥有自己的数据成员，意味着仿函数拥有状态；

#include
#include
using namespace std;

struct ADD {
	int num;
	ADD(int a){  //构造函数
		num = a;
	}
	int operator()(int x) {  //重载括号运算符
		return x + num;
	}
};

struct small {
	bool operator()(int a, int b) {
		return a < b; //如果第一个数小于第二个数返回true，排序是从小到大排列。返回真第一个参数的优先级高。
	}
};

class myclass
{
public:
	myclass(int x) : x_(x) {};
	int operator()(const int n) const { //这里加上const比较好，保护const int n，n不会被改变
		return n * x_;
	}
private :
	int x_;
};

int main(int argc, char** argv)
{
	// 1 struct结构体仿函数
	ADD add1(1);
	ADD add23(23);
	cout<<add23(55) << endl; //78
	cout<<add1(55) << endl;  //56
	cout << "-----------" << endl;
	// 2 匿名对象排序
	cout<<ADD(11).num << endl;  //ADD(11)相当于是匿名对象，也可以携程 a = ADD(11),a.num
	//匿名对象：不定义直接调用
	int a[] = { -4, -8, 2, -9,0 };
	int len = sizeof(a) / sizeof(a[0]);
	//sort(a, a + len, small());  //这里small()也是匿名对象（括号不要忘了）, 这句需要加上include
	sort(a, a + len, less<int>());  //stl自带，从小到大
	sort(a, a + len, greater<int>());  //stl自带，从大到小
	for(int i=0;i<=len-1;i++){ 
		cout << a[i] << endl;  //a
	}
	cout << "-----------" << endl;
	// 3 class类仿函数
	myclass C(2);
	cout << C(10) << endl;

	return 0;
}