SLAM-g2o学习

莱茶荼菜

已于 2025-04-24 14:57:07 修改

阅读量1.2k

点赞数 27

文章标签：机器人人工智能

于 2025-01-10 19:10:33 首次发布

本文链接：https://blog.youkuaiyun.com/qq_44691564/article/details/145058586

版权

本文是学习笔记，原文来自计算机视觉life

g2o在gitHub官网：https://github.com/RainerKuemmerle/g2o
g2o求解曲线参数的代码：https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

背景

拓展卡尔曼滤波

SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。不过，目前SLAM研究的主流热点几乎都是基于图优化的。
滤波方法尤其是EKF方法，在SLAM发展很长的一段历史中一直占据主导地位，它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

图优化

在图优化里，Bundle Adjustment（后面简称BA）起到了核心作用。但是之前SLAM的研究者们发现包含大量特征点和相机位姿的BA计算量其实很大，根本没办法实时。
后来SLAM研究者们发现了其实在视觉SLAM中，虽然包含大量特征点和相机位姿，但其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！比较代表性的就是2009年，几个大神发表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！

在SLAM里，图优化一般分解为两个任务：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

在图优化中，边的约束通常来自于：

里程计数据（相对位姿变化）

观测数据（如地标、特征点）

回环检测（机器人回到已知位置时）

外部传感器（如IMU、相机、激光雷达）

环境中的几何特征（如平面、直线等）

图优化结果示例

SparseOptimizer

首先看一下下面这个图，是g2o的基本框架结构。
在这里插入图片描述
我们先来看上面的结构吧。注意看 has-many 箭头，你看这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge

整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（我们常用的是GN和LM）

g2o的整个框架就是按照下图中标的这个顺序来写的。
在这里插入图片描述

迭代策略

高斯牛顿法

优化问题中我们的目标是：找到一个变量x使得目标函数f(x)最小化。

Hessian 矩阵是目标函数 f(x)对变量 x 的二阶偏导数构成的矩阵，定义如下：
在这里插入图片描述

在这里插入图片描述
在极值点时
$f'(x)=\triangledown f({x}_{k})+H({x}_{k})(x-{x}_{k})=0$

解得：
${x}_{k+1} = {x}_{k}-{H{X}_{K}}^{-1}\bigtriangledown f({x}_{k})$

准Newton 方法：在实际问题中，直接计算和存储 Hessian 矩阵可能过于昂贵。因此，准Newton 方法（如 BFGS 或 L-BFGS）通过近似 Hessian 矩阵或其逆矩阵，降低计算复杂度，同时保持高效性。

优势与限制

优势：
利用二阶导数信息，优化过程对目标函数曲率更加敏感，可以更快地收敛到极值。
特别适用于具有良好曲率结构的函数，能够准确找到极值点。

限制：
对于高维问题，计算 Hessian 矩阵的时间和内存复杂度很高。Hessian 矩阵可能不是正定的（例如目标函数的某些区域是凹的），需要特殊处理，如修正Hessian 矩阵或采用拟Hessian 近似。

注：凸函数的曲面是“开口向上”的，凹函数的曲面是“开口向下”的。

LM算法

Levenberg-Marquardt (LM) 是一种优化算法，用于解决非线性最小二乘问题，特别是在机器学习和数据拟合任务中。它结合了梯度下降法和高斯-牛顿法的优点，提供了一种在全局收敛性和局部优化效率之间取得平衡的方法。

核心思想
LM 算法通过在梯度下降和高斯-牛顿之间切换调整搜索方向，从而更有效地最小化非线性问题的残差平方和。其数学核心是解决下面的问题：
在这里插入图片描述

算法步骤

在这里插入图片描述

初始条件设置：

初始参数x0
阻尼因子 λ 用于平衡算法的两种模式

构建更新公式：在每次迭代中，LM 的参数更新公式如下：
调整阻尼因子 λ
如果本次迭代减少了目标函数值（即误差降低），减小 λ（让搜索方向更接近高斯-牛顿法）。
如果本次迭代增加了目标函数值，增大 λ（让搜索方向更接近梯度下降法）。
更新参数：使用新的 Δx 更新 x。
检查收敛条件：如果满足预设的收敛准则（如参数变化量或误差减小量小于阈值），停止迭代；否则返回第 2 步。

优点

结合了梯度下降法的全局收敛性和高斯-牛顿法的快速局部收敛性。
对于模型中的初始参数选择不敏感。
在小数据集的非线性最小二乘问题中非常高效。

缺点

需要计算雅可比矩阵，可能导致计算量较大。
对于非常高维度的优化问题，其效率可能不如其他方法。

应用场景

机器学习模型训练：非线性模型参数的优化，如神经网络训练（在某些早期应用中）。
数据拟合：科学计算中拟合实验数据，例如非线性回归。
图像处理：目标匹配或形状拟合。
总结来说，Levenberg-Marquardt 算法是一种稳健且高效的非线性优化工具，尤其适用于需要快速收敛的中小规模问题。

推导过程

在这里插入图片描述

代码及解析

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 表示变量的维度是3，观测的维度是1

// 创建一个稠密线性求解器实例，并将其分配给Block类型的线性求解器指针linearSolver。
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); 

// 第2步：创建一个BlockSolver实例，并将其与指定的线性求解器关联起来
Block* solver_ptr = new Block( linearSolver );      

// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化；创建了一个Levenberg-Marquardt算法实例，并将其与BlockSolver关联起来。

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
// 这是g2o库中的核心类，表示一个稀疏优化器。它用于构建和优化图模型，支持多种优化算法（如高斯-牛顿法、Levenberg-Marquardt法等）。
g2o::SparseOptimizer optimizer;     
optimizer.setAlgorithm( solver );   // 设置求解器
// setVerbose：这是SparseOptimizer类的一个方法，用于控制优化器的输出详细程度。
true：表示开启详细输出模式。在优化过程中，优化器会打印出每一步的优化信息，包括目标函数值的变化、迭代次数等。这对于调试和监控优化过程非常有帮助。
optimizer.setVerbose( true );       

// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) ); // 表示一个三参数的曲线拟合问题（例如，二次多项式 a x^2 +bx+c）。
v->setId(0);
optimizer.addVertex( v ); // 将顶点添加到优化器中。
for ( int i=0; i<N; i++ )    // 循环创建边
{
  CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
  edge->setId(i);                         // 设置边的ID
  edge->setVertex( 0, v );                // 设置边连接的顶点
  edge->setMeasurement( y_data[i] );      // 设置观测值
  edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma * w_sigma)); // 设置信息矩阵：协方差矩阵之逆
  optimizer.addEdge( edge );              // 将边添加到优化器中
}

// 第6步：设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);

1、创建一个线性求解器LinearSolver

我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。此时我们就需要一些特殊的方法对矩阵进行求逆，下图是GitHub上g2o相关部分的代码：
在这里插入图片描述

LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS LinearSolverPCG：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen：依赖项只有eigen，使用eigen中sparse Cholesky求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
Cholesky分解：是一种将对称正定矩阵分解为一个下三角矩阵和其转置的乘积的方法。具体来说，对于对称正定矩阵 A，可以分解为 A=LL^T，其中 L 是下三角矩阵，因此可以通过前向代入法快速求逆。
稀疏Cholesky分解：是指针对稀疏矩阵进行的Cholesky分解。由于稀疏矩阵的特殊性，这种分解方法会考虑如何减少非零元素的生成（fill-in）以及优化计算效率。

2.创建BlockSolver。并用上面定义的线性求解器初始化。

BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：

g2o/g2o/core/block_solver.h

你点进去会发现 BlockSolver有两种定义方式:一种是指定的固定变量的solver，我们来看一下定义

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度

另一种是可变尺寸的solver，因为在某些应用场景，我们的Pose和Landmark在程序开始时并不能确定，那么此时这个块状求解器就没办法固定变量，此时使用这个可变尺寸的solver，所有的参数都在中间过程中被确定，定义如下：

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

block_solver.h的最后，预定义了比较常用的几种类型，如下所示：

BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale BlockSolver_3_2：表示pose 是3维，观测点是2维

3. 创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化

我们来看g2o/g2o/core/ 目录下，发现Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下图所示，也和前面的图相匹配
总之，在该阶段，我们可以选则三种方法：

g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg

4、创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。

创建稀疏优化器

g2o::SparseOptimizer    optimizer;

用前面定义好的求解器作为求解方法：

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

其中setVerbose是设置优化过程输出信息用的

SparseOptimizer::setVerbose(bool verbose)

不信我们来看一下它的定义
在这里插入图片描述

5、定义图的顶点和边。并添加到SparseOptimizer中

6、设置优化参数，开始执行优化。

设置SparseOptimizer的初始化、迭代次数、保存结果等。

初始化

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

设置迭代次数，然后就开始执行图优化了。

SparseOptimizer::optimize(int iterations, bool online)