SLAM算法与工程实践——SLAM基本库的安装与使用(5):Ceres优化库

2023-12-16 12:10:23

SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接,本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接


下面是专栏地址:

SLAM算法与工程实践系列专栏



前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践


SLAM算法与工程实践——SLAM基本库的安装与使用(5):Ceres优化库

安装 Ceres

安装参考:

Ubuntu18.04安装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用法有如下几项:

  1. 定义残差块的类。方法是书写一个类(或结构体),并在类中定义带模板参数的()运算
    符,这样该类就成为了一个拟函数(Functor)。这种定义方式使得Ceres可以像调用函数一样,对该类的某个对象(比如 a )调用 a<double>() 方法。事实上,Ceres 会把雅可比矩阵作为类型参数传入此函数,从而实现自动求导的功能。

  2. 程序中的 double abc[3] 即参数块,而对于残差块,我们对每一个数据构造 CURVE FITTING COST 对象,然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度,我们有若干种选择:

    (1)使用Ceres的自动求导(Auto Diff);

    (2)使用数值求导(Numeric Diff);

    (3)自行推导解析的导数形式,提供给Ceres。因为自动求导在编码上是最方便的,于是我们使用自动求导。

  3. 自动求导需要指定误差项和优化变量的维度。这里的误差是标量,维度为1:优化的是 a,b,c 三个量,维度为3。于是,在自动求导 AutoDiffCostFunction 的模板参数中设定变量维度为1、3。

  4. 设定好问题后,调用Solve函数进行求解。你可以在 options 里配置(非常详细的)优化选项。例如,可以选择使用 Line Search 还是 Trust Region 、迭代次数、步长,等等。读者可以查看 Options 的定义,看看有哪些优化方法可选,当然默认的配置已经可用于很广泛的问题了。

入门介绍

参考:

http://ceres-solver.org/nnls_modeling.html?highlight=autodiffcostfunction#_CPPv4N5ceres20AutoDiffCostFunctionE

ceres的常见用法总结

优化框架Ceres库用法介绍

ceres库的使用

Ceres-Solver 从入门到上手视觉SLAM位姿优化问题

【SLAM】Ceres优化库超详细解析

Ceres详解(一) Problem类

【SLAM】Ceres优化库官方实例详细解析(SLAM相关问题)

Ceres Solver:从入门到使用

非线性优化器ceres的使用20221125

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?)?i1,?,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' minrTr

使用 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_residualsparameter_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);

文章来源:https://blog.csdn.net/qq_37648371/article/details/135006573
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。