SLAM算法与工程实践——SLAM基本库的安装与使用(5):Ceres优化库
SLAM算法与工程实践系列文章
下面是SLAM算法与工程实践系列文章的总链接,本人发表这个系列的文章链接均收录于此
SLAM算法与工程实践系列文章链接
下面是专栏地址:
SLAM算法与工程实践系列专栏
文章目录
前言
这个系列的文章是分享SLAM相关技术算法的学习和工程实践
SLAM算法与工程实践——SLAM基本库的安装与使用(5):Ceres优化库
安装 Ceres
安装参考:
Ubuntu 20.04可以按照官网的方法来安装最新版本,Ubuntu 18.04则要安装老版本的Ceres 1.14.0,见上面的教程
官网:http://ceres-solver.org/
下载源码
You can start with the latest stable release . Or if you want the latest version, you can clone the git repository
git clone https://ceres-solver.googlesource.com/ceres-solver
安装依赖
# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# Use ATLAS for BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# Eigen3 ,若之前安装过就不用再装了
sudo apt-get install libeigen3-dev
# SuiteSparse (optional)
sudo apt-get install libsuitesparse-dev
编译源码
tar zxf ceres-solver-2.1.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-2.1.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.
sudo make install
安装完成后,可以在 /usr/local/include/ceres 下找到Ceres的头文件,并在 /usr/local/lib/ 下找到名为 libceres.a
的库文件。如果能找到就代表安装成功了。
错误
参考:
Using ceres-solver do Bundle Adjustment Runing Crash #428
在输入命令 cmake ..
后,报如下错误
-- Failed to find installed glog CMake configuration, searching for glog build directories exported with CMake.
-- Failed to find an installed/exported CMake configuration for glog, will perform search for installed glog components.
-- Failed to find glog - Could not find glog include directory, set GLOG_INCLUDE_DIR to directory containing glog/logging.h
CMake Error at CMakeLists.txt:410 (message):
Can't find Google Log (glog). Please set either: glog_DIR (newer CMake
built versions of glog) or GLOG_INCLUDE_DIR & GLOG_LIBRARY or enable
MINIGLOG option to use minimal glog implementation.
使用
参考:
Ceres的Problem::AddParameterBlock()函数
http://ceres-solver.org/nnls_modeling.html#theory
ceresCurveFitting.cpp
这里以 ceresCurveFitting.cpp
为例
首先引入头文件,其中 chrono 库是C++标准库中的一个组件,用于表示和处理时间
详细可见:C++ std::chrono库使用指南 (实现C++ 获取日期,时间戳,计时等功能)
#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>
进入主函数,首先是随机生成100个加了噪声的点,将其 pushback 进 vector 中
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
double inv_sigma = 1.0 / w_sigma;
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
}
然后定义了我们要估计的变量,将其存入数组中
double abc[3] = {ae, be, ce};
接着构建最小二乘问题
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导,模板参数:误差类型,输出维度,输入维度,维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
new CURVE_FITTING_COST(x_data[i], y_data[i])
),
nullptr, // 核函数,这里不使用,为空
abc // 待估计参数
);
}
其中 CURVE_FITTING_COST
结构体定义为:
// 代价函数的计算模型
struct CURVE_FITTING_COST {
CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {} // 有参构造,这里的构造函数为空
// 残差的计算
template<typename T>
// 对括号()运算符进行重载,是给Ceres内部求导用的
bool operator()(
const T *const abc, // 模型参数,有3维
T *residual) const {
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
关于 AutoDiffCostFunction ,详见:
http://ceres-solver.org/nnls_modeling.html?highlight=autodiffcostfunction#_CPPv4N5ceres20AutoDiffCostFunctionE
配置求解器,用优化库来求解
// 配置求解器
ceres::Solver::Options options; // 这里有很多配置项可以填
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解
options.minimizer_progress_to_stdout = true; // 输出到cout
ceres::Solver::Summary summary; // 优化信息
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve(options, &problem, &summary); // 开始优化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
输出结果
// 输出结果
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for (auto a:abc) cout << a << " ";
cout << endl;
执行后输出为
程序中需要说明的地方均已加注释。可以看到,我们利用 OpenCV 的噪声生成器生成了 100 个带高斯噪声的数据,随后利用Ceres进行拟合。这里演示的Ceres用法有如下几项:
-
定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的
()
运算
符,这样该类就成为了一个拟函数(Functor)。这种定义方式使得Ceres可以像调用函数一样,对该类的某个对象(比如 a )调用 a<double>() 方法。事实上,Ceres 会把雅可比矩阵作为类型参数传入此函数,从而实现自动求导的功能。 -
程序中的 double abc[3] 即参数块,而对于残差块,我们对每一个数据构造 CURVE FITTING COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,我们有若干种选择:
(1)使用Ceres的自动求导(Auto Diff);
(2)使用数值求导(Numeric Diff);
(3)自行推导解析的导数形式,提供给Ceres。因为自动求导在编码上是最方便的,于是我们使用自动求导。
-
自动求导需要指定误差项和优化变量的维度。这里的误差是标量,维度为1:优化的是 a,b,c 三个量,维度为3。于是,在自动求导 AutoDiffCostFunction 的模板参数中设定变量维度为1、3。
-
设定好问题后,调用Solve函数进行求解。你可以在 options 里配置(非常详细的)优化选项。例如,可以选择使用 Line Search 还是 Trust Region 、迭代次数、步长,等等。读者可以查看 Options 的定义,看看有哪些优化方法可选,当然默认的配置已经可用于很广泛的问题了。
入门介绍
参考:
http://ceres-solver.org/nnls_modeling.html?highlight=autodiffcostfunction#_CPPv4N5ceres20AutoDiffCostFunctionE
Ceres-Solver 从入门到上手视觉SLAM位姿优化问题
【SLAM】Ceres优化库官方实例详细解析(SLAM相关问题)
Ceres库主要用于求解无约束或者有界约束的最小二乘问题。其数学形式如下:
min
?
x
1
2
∑
i
ρ
i
(
∥
f
i
(
x
1
,
?
?
,
x
k
)
∥
2
)
s
.
t
.
l
j
≤
x
j
≤
u
j
\begin{aligned}\min_x&\quad\frac12\sum_i\rho_i\left(\|f_i(x_1,\cdots,x_k)\|^2\right)\\\mathrm{s.t.}&\quad l_j\leq x_j\leq u_j\end{aligned}
xmin?s.t.?21?i∑?ρi?(∥fi?(x1?,?,xk?)∥2)lj?≤xj?≤uj??
我们的任务就是找到一组满足约束
l
j
≤
x
j
≤
u
j
l_j ≤ x_j ≤ u _j
lj?≤xj?≤uj? 的
x
1
,
?
?
,
x
k
x_1,\cdots,x_k
x1?,?,xk? ,使得优化目标函数
1
2
∑
i
ρ
i
(
∥
f
i
(
x
1
,
?
?
,
x
k
)
∥
2
)
\frac{1}{2}\sum_{i}\rho_{i}\left(\|f_{i}\left(x_{1},\cdots,x_{k}\right)\|^{2}\right)
21?i∑?ρi?(∥fi?(x1?,?,xk?)∥2)
取值最小
其中的一些变量的解释:
- 优化参数
x
1
,
?
?
,
x
k
x_1,\cdots,x_k
x1?,?,xk? :
参数块
(ParameterBlock),它们的取值就是我们要寻找的解 - l j , u j l_j,u_j lj?,uj? :第 j j j 个优化参数 x j x_j xj? 的下界和上界
- 表达式
ρ
(
∥
f
i
(
x
1
,
?
?
,
x
k
)
∥
2
)
\rho\big(\|f_i(x_1,\cdots,x_k)\|^2\big)
ρ(∥fi?(x1?,?,xk?)∥2) :
残差块
(ResidualBlock) -
f
i
(
?
)
f_i(\cdot)
fi?(?) :
代价函数
(CostFunction) -
ρ
i
(
?
)
\rho_i(\cdot)
ρi?(?) :
损失函数
,或者核函数
(LossFunction)。它的意义主要是为了降低野点
(outliers)对于解的影响
代价函数同时负责计算残差项 (
f
i
(
x
1
,
?
?
,
x
k
)
f_i(x_1,\cdots,x_k)
fi?(x1?,?,xk?))和雅可比矩阵
J
i
\mathcal{J_i}
Ji?
J
i
=
?
?
x
i
f
i
(
x
1
,
?
?
,
x
k
)
?
i
∈
1
,
?
?
,
k
\mathcal{J}_{i}=\frac{\partial}{\partial x_{i}}f_{i}(x_{1},\cdots,x_{k})\quad\forall i\in1,\cdots,k
Ji?=?xi???fi?(x1?,?,xk?)?i∈1,?,k
很多时候我们说最小二乘都是拿来做曲线拟合的,实际上只要能够把问题描述成上面的数学形式,就都可以使用Ceres来求解。使用起来也比较简单,只要按照教程介绍的套路,提供代价函数的计算方式,描述清楚每个残差块以及核函数即可。
如下面的示例代码所示,一般我们需要定义三个对象,problem
用于描述将要求解的问题,options
提供了很多配置项,而summary
用于记录求解过程。
ceres::Problem problem;
ceres::Solver::Options options;
ceres::Solver::Summary summary;
Ceres的求解过程包括构建最小二乘和求解最小二乘问题两部分,其中构建最小二乘问题的相关方法均包含在 Ceres::Problem 类中,涉及的成员函数主要包括 Problem::AddResidualBlock()
和 Problem::AddParameterBlock()
。
注意:
Ceres Solver 只接受最小二乘优化,也就是
min
?
r
T
r
\min r^{T} r
minrTr ;若要对残差加权重,使用马氏距离,即
min
?
r
T
P
?
1
r
\min r^{T}P^{-1} r
minrTP?1r ,
则要对信息矩阵
P
?
1
P^{-1}
P?1 做Cholesky分解,即
L
L
T
=
P
?
1
LL^T=P^{-1}
LLT=P?1 ,则
d
=
r
T
(
L
L
T
)
r
=
(
L
T
r
)
T
(
L
T
r
)
d=r^T(LL^T)r=(L^Tr)^T(L^Tr)
d=rT(LLT)r=(LTr)T(LTr) ,令
r
′
=
L
T
r
r'=L^Tr
r′=LTr ,最终
min
?
r
′
T
r
′
\min r'^Tr'
minr′Tr′
使用 Ceres Solver 求解非线性优化问题,主要包括以下几部分:
- 构建代价函数(cost function) 或 残差(residual)
- 构建优化问题(ceres::Problem):通过 AddResidualBlock 添加代价函数(cost function)、损失函数(loss function) 和 待优化状态量
LossFunction
: a scalar function that is used to reduce the influence of outliers on the solution of non-linear least squares problems.
- 配置求解器(ceres::Solver::Options)
- 运行求解器(ceres::Solve(options, &problem, &summary))
Ceres 使用流程
使用 Ceres Solver 求解非线性优化问题,主要包括以下几部分:
【STEP 1】构建优化问题(ceres::Problem)
【STEP 2】构建代价函数(ceres::CostFunction)或残差(residual)
【STEP 3】添加代价函数、核函数:通过ceres::Problem::AddResidualBlock添加代价函数(cost function)、核函数(loss function)和输入参数块(即待优化状态量块)
【STEP 4】配置求解器(ceres::Solver::Options)
【STEP 5】运行求解器(ceres::Solve(options, &problem, &summary))
构建代价函数(STEP2)
代价函数最重要的功能就是计算残差向量和雅可比矩阵。Ceres提供了三种求导方法,分别是:解析求导、自动求导、数值求导。
在SLAM中,使用的一般都是解析求导,这种方法需要自己填入雅克比函数。
基础代价函数类
先看下最基础的代价函数类的声明:
class CostFunction {
public:
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const = 0;
const vector<int32>& parameter_block_sizes();
int num_residuals() const;
protected:
vector<int32>* mutable_parameter_block_sizes();
void set_num_residuals(int num_residuals);
};
CostFunction的输入参数块的数量和大小以及输出的数量,分别存储在CostFunctio::parameter_block_sizes_
和CostFunction::num_residuals_
中。从此类继承的子类应使用对应的访问器设置这两个成员。
当使用Problem::AddResidualBlock()
添加代价函数时,此信息将由Problem验证。
CostFunction中有一个纯虚函数需要子类进行重写,该纯虚函数的定义为:
bool CostFunction::Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const;
纯虚函数的形参的解释:
- parameters:输入参数块,大小为
parameter_block_sizes_.size()
的数组(输入参数块的数量),parameters[i]
是大小为parameter_block_sizes_[i]
的数组(第i个输入参数块的维度) residuals
:残差,大小为num_residuals_
的数组jacobians
:雅可比矩阵,大小为parameter_block_sizes_.size()
的数组的数组(输入参数块的数量),jacobians[i]
是大小为num_residuals
乘parameter_block_sizes_[i
的行优先数组(残差对第i个输入参数块求导,即大小为残差维度 乘 第i个输入参数块维度,再转化为行优先数据组)
jacobians的详细表达式为:
jacobians
[
i
]
[
r
?
parameter?block?sizes
?
[
i
]
+
c
]
=
?
residual
[
r
]
?
parameters
[
i
]
[
c
]
\text{jacobians}[\mathbf{i}][\mathbf{r}^*\text{parameter block sizes}_-[\mathbf{i}]+\mathbf{c}]=\frac{\partial\text{residual}[r]}{\partial\text{parameters}[i][c]}
jacobians[i][r?parameter?block?sizes??[i]+c]=?parameters[i][c]?residual[r]?
parameters 和 residuals 不能为 nullptr,jacobians 可能为 nullptr。如果 jacobians 为 nullptr,则用户只需计算残差即可。
纯虚函数的返回值指示残差或雅可比的计算是否成功。
1. 定义残差函数
残差函数是用户自定义的模型函数与观测数据之间的误差函数,是Ceres库求解优化问题的重要部分
struct CostFunctor {
template
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
以上代码定义了一个求解 x 使得10-x=0 的问题。
2. 定义优化问题
在Ceres库中,优化问题包含一个或多个参数块(Parameter Block)、一个或多个残差函数(Cost Function)、以及优化参数的起始值。在定义优化问题时,需要将所有的优化数据加入到problem对象中。
// 定义优化问题
ceres::Problem problem;
// 添加参数块
double initial_x = 5.0;
double x = initial_x;
problem.AddParameterBlock(&x, 1);
// 添加残差函数
CostFunctor* cost_functor = new CostFunctor;
ceres::CostFunction* cost_function =
new ceres::AutoDiffCostFunction(cost_functor);
problem.AddResidualBlock(cost_function, nullptr, &x);
// 设置优化参数的起始值
ceres::Solver::Options options;
options.initial_trust_region_radius = 1e-10;
// 求解问题
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
3. Ceres库的求解器
Ceres库提供了多种优化问题求解器,包括本地方法(Local Method)和迭代方法(Iterative Method),用户可以根据自己的需要进行选择。
本地方法
Local Parameterization 可以对参数块进行变换,旋转或者平移等,然后在新的参数块上进行非线性优化。
// 定义旋转参数块
double rotation[4] = {1.0, 0.0, 0.0, 0.0};
ceres::LocalParameterization* quaternion_parameterization =
new ceres::QuaternionParameterization;
problem.AddParameterBlock(rotation, 4, quaternion_parameterization);
// 建立优化问题
ceres::CostFunction* cost_function = new MyCostFunction;
ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
// 通过问题中添加参数块
ceres::ResidualBlockId residual_block_id = problem.AddResidualBlock(
cost_function, loss_function, rotation, translation);
// 求解问题
ceres::Solver::Options options;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
迭代方法
Ceres库提供了多种的迭代方法,如 “L-BFGS-B”, “Trust Region Reflective”, “Dogleg”, “CG” 等,其中每种方法在 不同场景下都体现了特殊的优势。
ceres::GradientProblemSolver::Options options;
options.line_search_direction_type = ceres::BFGS;
options.line_search_type = ceres::WOLFE;
options.max_num_iterations = 200;
options.minimizer_progress_to_stdout = true;
ceres::GradientProblemSolver::Summary summary;
ceres::Solve(options, &problem, &summary);
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!