SLAM学习——Ceres
一、安装配置
- 依赖
# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse and CXSparse (optional)
# - If you want to build Ceres as a *static* library (the default)
# you can use the SuiteSparse package in the main Ubuntu package
# repository:
sudo apt-get install libsuitesparse-dev
- 安装
tar zxf ceres-solver-1.14.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-1.14.0
make -j3
make test
# Optionally install Ceres, it can also be exported using CMake which
# allows Ceres to be used without requiring installation, see the documentation
# for the EXPORT_BUILD_DIR option for more information.
make install
- CMakeLists.txt配置
# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
# 添加CERES_LIBRARIES依赖
add_executable(main # 输出名为main的可执行文件
./src/main.cpp
)
target_link_libraries( main ${CERES_LIBRARIES} )
二、使用
求解步骤
- 定义Cost Function(损失函数)模型，也就是寻优的目标式。这个部分使用仿函数（functor）来实现，做法是定义一个Cost Function的结构体，在结构体内重载()运算符。
- 通过代价函数构建待求解的优化问题。
- 配置求解器参数并求解问题，这个步骤就是设置方程怎么求解、求解过程是否输出等，然后调用一下Solve方法。
2.1 构造代价函数结构体
2.1.1 自动求导
自动求导构造误差函数变量需要全部为T类型，operator()为模板函数。
一次函数
y = 1 2 ⋅ ( 10 − x ) 2 y = \frac{1}{2} \cdot (10-x)^2 y=21⋅(10−x)2
求 x x x使得 y y y最小（最接近0）
struct CostFunctor
{
template
bool operator()(const T *const x,// 模型参数，一维
T *residual) const // 残差
{
residual[0] = T(10.0) - x[0];
return true;
}
};
曲线拟合
y = e x p ( a x 2 + b x + c ) + w y = exp(ax^2+bx+c)+w y=exp(ax2+bx+c)+w
假设有一条满足该方程的曲线，其中 a , b , c a,b,c a,b,c为曲线参数， w w w为噪声(理想条件下为0)
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] = T(_y) - ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
return true;
}
const double _x,_y;
};
2.1.2 数值求导
数值求导得误差函数变量不需要变换为T类型。
一次函数
struct NumericDiffCostFunctor {
bool operator()(const double* const x, double* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
2.1.3 解析导数
某种情况下，在封闭形式下计算导数比自动微分依赖的链式法则计算更有效。在这种情况下，需要你自己提供计算剩余函数和Jacobian矩阵的代码，根据编译时参数和剩余量(residuals)的维数定义CostFunction或SizedCostFunction的子类。
注意：Jacobian矩阵为误差函数关于优化参数的偏导
链式法则，连锁律、链式法则或链锁定则（英语：chain rule），是求复合函数导数的一个法则：
y = f ( g ( x ) ) ⇔ y = f ( u ) , u = g ( x ) d y d x = d y d g d g d x y = f(g(x))\Leftrightarrow y =f(u),u = g(x)\\ \frac{dy}{dx} = \frac{dy}{dg}\frac{dg}{dx} y=f(g(x))⇔y=f(u),u=g(x)dxdy=dgdydxdg
定义
template // Number of parameters in block 9.
class AutoDiffCostFunction : public
SizedCostFunction {
public:
explicit AutoDiffCostFunction(CostFunctor* functor);
// Ignore the template parameter kNumResiduals and use
// num_residuals instead.
AutoDiffCostFunction(CostFunctor* functor, int num_residuals);
};
kNumResiduals为残差个数，N0,N1...参数块，数字的大小代表参数块包含的参数的个数
virtual bool Evaluate(
double const* const* parameters,
double* residuals,
double** jacobians)
const { }
parameters二维参数块指针，和SizedCostFunction设置的参数块对应residuals一维残差指针jacobians二维jacobian矩阵元素指针，与parameters对应，每个参数块对应一组jacobian，每组jacobian元素的数目顺序必须与对应参数块参数个数顺序一致，即jacobian[i][j]应为parameters[i][j]的偏导
一次函数
e r r = 10 − p err = 10 - p err=10−p
优化参数为p，对p求偏导即可
f ( p ) ′ = − 1 {f(p)}' = -1 f(p)′=−1
//第一个参数为残差个数，第二个开始一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class QuadraticCostFunction: public ceres::SizedCostFunction<1,1> {
public:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(
double const* const* parameters,//二维参数块指针
double* residuals, //一维残差指针
double** jacobians) //二维jacobian矩阵元素指针,与parameters对应
const {
residuals[0] = 10.0 - parameters[0][0];
// Compute the Jacobian if asked for.
if(jacobians && jacobians[0]) {
jacobians[0][0] = -1;
}
return true;
}
};
QuadraticCostFunction::Evaluate包含了输入变量parameters、输出剩余值residuals和雅各比矩阵jacobians，其中jacobians 为可选参数，如果非空，则在函数Evaluate 中会计算剩余函数的导数值。在以上的例子中，由于剩余函数为线性函数，所以雅各比是一个常数。
曲线拟合
e r r = e x p ( a x 2 + b x + c ) − y err = exp(ax^2+bx+c)-y err=exp(ax2+bx+c)−y
这里优化参数为 a , b , c a,b,c a,b,c， x , y x,y x,y为已知数据（这和定义的公式不一样），分别计算误差函数关于三个参数的偏导：
f ( a ) ′ = x 2 e x p ( a x 2 + b x + c ) f ( b ) ′ = x e x p ( a x 2 + b x + c ) f ( c ) ′ = e x p ( a x 2 + b x + c ) f(a)' = x^2 exp(ax^2+bx+c)\\ f(b)' = x exp(ax^2+bx+c)\\ f(c)' = exp(ax^2+bx+c) f(a)′=x2exp(ax2+bx+c)f(b)′=xexp(ax2+bx+c)f(c)′=exp(ax2+bx+c)
//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class CurveFittingCostFunction : public ceres::SizedCostFunction<1, 3>
{
public:
CurveFittingCostFunction(double x, double y) : _x(x), _y(y) {}
virtual ~CurveFittingCostFunction() {}
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const
{
double a = parameters[0][0];
double b = parameters[0][1];
double c = parameters[0][2];
residuals[0] = ceres::exp(a * _x * _x + b * _x + c) - _y;
// Compute the Jacobian if asked for.
if (jacobians != NULL && jacobians[0] != NULL)
{
jacobians[0][0] = _x * _x * ceres::exp(a * _x * _x + b * _x + c);
jacobians[0][1] = _x * ceres::exp(a * _x * _x + b * _x + c);
jacobians[0][2] = ceres::exp(a * _x * _x + b * _x + c);
}
return true;
}
private:
double _x, _y;
};
2.2 构建待求解的优化问题
Problem problem;
CostFunction* cost_function =
new AutoDiffCostFunction(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
- 设置优化梯度，可选项为：
-
Ceres 的自动求导（Auto Diff）（最简单）
CostFunction* cost_function = new AutoDiffCostFunction自动求导需要指定误差项和优化变量的维度，此处误差为标量，维度为1，一次函数优化值维度为1，设置为(1,1)，曲线拟合优化值维度为3，设置为(1,3)。
-
数值求导（Numeric Diff）
CostFunction* cost_function = new NumericDiffCostFunction一般比自动求导法收敛更慢，且更容易出现数值错误，其构造上相比自动求导法多出一个模板参数
ceres::CENTRAL。 -
自行推导解析的导数形式，提供给Ceres。
QuadraticCostFunction继承自SizedCostFunction，而SizedCostFunction继承自CostFunction，因此可以将解析导数的类直接赋值。
CostFunction* cost_function = new QuadraticCostFunction();
- 调用
AddResidualBlock将误差项添加到目标函数中
problem.AddResidualBlock(cost_function, nullptr, &x);
nullptr，核函数，可用于数据处理，比如去除离散点等。&x，待优化参数。
2.3 配置问题并求解
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; // 配置增量方程的解法
options.minimizer_progress_to_stdout = true; // 输出到cout
Solver::Summary summary; // 优化信息
Solve(options, &problem, &summary); // 开始优化
option，优化选项，涉及优化方法，迭代次数，步长等等，详细可参考solver-optionsSummary，优化信息
三、应用
3.1 解方程
#include
#include
using namespace std;
using namespace ceres;
// 第一部分：构建代价函数，重载()符号，仿函数的小技巧
// 求x使得1/2*(10-x)^2取到最小值
struct CostFunctor
{
template
bool operator()(const T *const x, // 模型参数，一维
T *residual) const // 残差
{
residual[0] = T(10.0) - x[0];
return true;
}
};
//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class lineCostFunction : public ceres::SizedCostFunction<1, 1>
{
public:
virtual ~lineCostFunction() {}
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const
{
residuals[0] = 10.0 - parameters[0][0];
// Compute the Jacobian if asked for.
if (jacobians != NULL && jacobians[0] != NULL)
{
jacobians[0][0] = -1;
}
return true;
}
};
int main(int argc, char **argv)
{
google::InitGoogleLogging(argv[0]);
// 一次函数，寻优参数x的初始值，为5
double initial_x = 20.0;
double x = initial_x;
// 第二部分：构建寻优问题i
Problem problem;
// 使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
CostFunction *cost_function =
new AutoDiffCostFunction(new CostFunctor);
// 解析导数
// CostFunction *cost_function = new lineCostFunction();
// 向问题中添加误差项，本问题比较简单，添加一个就行。
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";
}
3.2 曲线拟合
#include
#include
#include
using namespace std;
using namespace ceres;
// 第一部分：构建代价函数，重载()符号，仿函数的小技巧
// 拟合曲线
// 曲线方程: y = exp(ax^2+b^x+c)+w
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] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
return true;
}
const double _x, _y;
};
//残差个数，一个数字代表一个参数快，数字的大小代表参数快包含的参数的个数
class CurveFittingCostFunction : public ceres::SizedCostFunction<1, 3>
{
public:
CurveFittingCostFunction(double x, double y) : _x(x), _y(y) {}
virtual ~CurveFittingCostFunction() {}
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const
{
double a = parameters[0][0];
double b = parameters[0][1];
double c = parameters[0][2];
residuals[0] = ceres::exp(a * _x * _x + b * _x + c) - _y;
// Compute the Jacobian if asked for.
if (jacobians != NULL && jacobians[0] != NULL)
{
jacobians[0][0] = _x * _x * ceres::exp(a * _x * _x + b * _x + c);
jacobians[0][1] = _x * ceres::exp(a * _x * _x + b * _x + c);
jacobians[0][2] = ceres::exp(a * _x * _x + b * _x + c);
}
return true;
}
private:
double _x, _y;
};
int main(int argc, char **argv)
{
google::InitGoogleLogging(argv[0]);
// 拟合曲线
double a = 3, b = 2, c = 1, w = 1;
cv::RNG rng;
double abc[3] = {0, 0, 0};
vector x_data(1000), y_data(1000);
for (int i = 0; i < 1000; i++)
{
double x_tmp = i / 1000.0;
x_data[i] = x_tmp;
y_data[i] = ceres::exp(a * x_tmp * x_tmp + b * x_tmp + c) + rng.gaussian(w);
}
// 第二部分：构建寻优问题
Problem problem;
for (int i = 0; i < 1000; i++)
{
problem.AddResidualBlock(
new AutoDiffCostFunction(new CURVE_FITTING_COST(x_data[i], y_data[i])),
nullptr,
abc);
// 解析导数
/*problem.AddResidualBlock(
new CurveFittingCostFunction(x_data[i], y_data[i]),
nullptr,
abc);*/
}
// 第三部分： 配置并运行求解器
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 << "a : " << abc[0] <<
" b : " << abc[1] <<
" c : " << abc[2] << std::endl;
return 0;
}
3.3 鲁棒曲线拟合
求解优化问题中（比如拟合曲线），数据中往往会有离群点、错误值什么的，最终得到的寻优结果很容易受到影响，此时就可以使用一些损失核函数来对离群点的影响加以消除。要使用核函数，只需要把上述代码中的NULL或nullptr换成损失核函数结构体的实例。
Ceres库中提供的核函数主要有：TrivialLoss 、HuberLoss、 SoftLOneLoss 、 CauchyLoss。
比如此时要使用CauchyLoss，只需要将nullptr换成new CauchyLoss(0.5)就行（0.5为参数）
Problem problem;
for (int i = 0; i < 1000; i++)
{
problem.AddResidualBlock(
new AutoDiffCostFunction(new CURVE_FITTING_COST(x_data[i], y_data[i])),
new CauchyLoss(0.5),
abc);
}
四、补充
- 仿函数
仿函数(functor)，就是使一个类的使用看上去象一个函数。其实现就是类中实现一个operator()，这个类就有了类似函数的行为，就是一个仿函数类了。
此处实现Myclass类，重载operator()可实现仿函数，即建立类后，可以将类作为函数，类似Myclass()。
#include
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);
std::cout< 参考
Ceres Solver
Ceres入门
Ceres入门详解
Ceres-Solver库使用（三）–导数(Derivatives)
Ceres总结
非线性优化Ceres手动求导数值求导解析求导使用示例