G2O使用

最新推荐文章于 2024-07-08 22:12:26 发布

AI脚下的巨人

最新推荐文章于 2024-07-08 22:12:26 发布

阅读量782

点赞数 13

分类专栏： g2o 文章标签：算法

本文链接：https://blog.csdn.net/qq_20604231/article/details/136198362

版权

g2o 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

0. 简介

本文主要参考计算机视觉life 的文章整理了自己对g2o的使用总结,详见:

1. G2O 框架

在这里插入图片描述

1.1 优化问题

在讲G2O框架前先回想一下优化问题.一个优化问题涉及以下几个问题:

定义优化变量：待优化的变量,在图优化中对应顶点
定义优化目标:最小化误差,在图优化对应边
如何线性化:目标函数一般是非线性,需要指明使用什么方法线性化(雅可比矩阵)
计算 $H\Delta x=-b$ :由于SLAM的优化问题一般是稀疏的,求解这个线性方程组需要使用一些技巧

1.2 框架介绍

HyperGraph::Vertex 对应优化问题的顶点
HyperGraph::Edge 对应优化问题的边
核心SparseOptimizer
- 优化(迭代)算法:
  - Gauss-Newton(GN)
  - Levernberg-Marquardt(LM)
  - Powell’s dogleg
- 线性求解器(线性方程的稀疏求解方法)

1.3 基于G2O构建一个优化问题

1.3.1 创建一个线性求解器LinearSolver:解 $H\Delta x=-b$

g2o提供了以下几种线性方程组求解器：

LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen：依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver

1.3.2 根据上面定义的LinearSolver创建BlockSolver

BlockSolver可以定义为静态或动态

// 静态 p: pose 位姿; l: landmark 路标
g2o::BlockSolver< g2o::BlockSolverTraits<p,l> >
// 动态
using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

平常主要使用静态,g2o本身也提供以下几种常用的维度:

BlockSolver_6_3: pose-6维, 地图点-3维
BlockSolver_7_3: pose-7维(多了1维的scale)
BlockSolver_3_2: pose-3维(x/y/ $\theta$ ) , 地图点-2维,一般是二维slam

==疑惑?==:g2o::BlockSolverTraits的维度定义不是十分明确,在<14讲>中:

g2oCurveFitting.cpp例子,一元边,中landmark注释维误差维度

typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1

pose_estimation_3d2d.cpp例子,同样是一元边,但landmark又写成

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3

查了ChatGPT又说一元边可以忽略landmark这一参数(甚至可写为0)

1.3.3 创建总求解器solver

总求解器需要指明:迭代算法和上面的线性求解器

因此,首先根据迭代算法声明总求解器,有以下三种:

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

无论上面的那一种均继承了OptimizationWithHessian

通过上面的线性求解器初始化总求解器

// BlockSolver
Block* solver_ptr = new Block( linearSolver );    

// 总求解器
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );

1.3.4 创建稀疏优化器

g2o::SparseOptimizer    optimizer;

// 稀疏优化器设置上面定义好的求解器
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
// 可选择打印调试细腻系
SparseOptimizer::setVerbose(bool verbose)

1.3.5 顶点和边以及开始优化

// 往优化器添加顶点和边
...
// 下文讲解

// 初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
// 开始图优化
SparseOptimizer::optimize(int iterations, bool online)

2. G2O定义顶点

2.1 顶点的模板参数

BaseVertex<D, T>:

D: static const int Dimension = D; 顶点(待优化变量)的维度
T: typedef T EstimateType; 顶点的类型

一般创建一个类继承BaseVertex,例如

class myVertex: public g2o::BaseVertex<D, T> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void read(std::istream& is) {}
    virtual void write(std::ostream& os) const {}

    virtual void setOriginImpl();
    virtual void oplusImpl(const double* update) override;
};

2.2 重载成员方法

oplusImpl
通过求解线性方程组后我们会得到状态量的增量 $\Delta x$ ,有了增量的下一步是将其叠加到现在的状态上.
如果状态量是简单的,不涉及广义的加法,则直接参照向量叠加就可以了.但SLAM的问题中离不开旋转,SO3和SE3都不能直接相加,所以g2o留有oplusImpl便于我们重载以实现状态量的更新
setToOriginImpl
这个只是用来重置状态量,一般给0置或者单位矩阵即可
read和wirte
这两个成员方法是用来存盘和读盘, 一般不需要,直接留空即可

2.3 往图中添加顶点

添加顶点步骤:

创建顶点;
设置顶点初值(给定状态量的初值便于优化);
设置顶点ID:setId(int);
往图添加顶点:addVertex

例子

// 往图中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );

3. G2O定义边

3.1 边的种类

在这里插入图片描述

种类:

BaseUnaryEdge: 一元边
BaseBinaryEdge: 两元边
BaseMultiEdge: 多元边

3.2 边的模板参数

以重投影误差的边为例

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

2: 测量值维度为2,二维平面的u/v观测值
Vector2D: 测量值的类型
由于是二元边,所以涉及2个顶点:
- VertexSBAPointXYZ: 地图点(空间3维点)
- VertexSE3Expmap: 相机的位姿变换关系

3.3 重载成员方法

和顶点同理,需要重载边的成员方法

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

read和wirte 这两个成员方法是用来存盘和读盘,同样可以留空;
computeError,定义误差的计算方式;
linearizeOplus,定义误差关于状态量的偏导数(雅可比矩阵).

不难可出,边的关键就是computeError和linearizeOplus,它将直接影响优化的成功与否

3.4 成员变量

_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆

除此以外,还可以根据自己需要新增其它成员变量便于计算误差

3.5 往图中添加边

添加步骤:

创建边;
setId:设置边的ID;
setVertex设置边的顶点;
setMeasurement设置边的测量值;
setInformation设置信息矩阵(协方差的逆);
addEdge 往图添加边

以<14讲>中的3d_2d为例

for ( const Point2f p:points_2d )
{
    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId ( index );
    edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
    edge->setVertex ( 1, pose );
    edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
    edge->setParameterId ( 0,0 );
    edge->setInformation ( Eigen::Matrix2d::Identity() );
    optimizer.addEdge ( edge );
    index++;
}