【自看】g2o 库函数

揣在兜里的黑桃k

已于 2023-05-25 13:43:48 修改

阅读量340

点赞数

文章标签：算法

于 2023-05-15 11:24:47 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_52867897/article/details/130679478

版权

图优化理论

图优化，就是把优化问题表现成图(Graph)的一种方式，一个图由若干顶点(Vertex)以及连接这些顶点的边(Edge)构成。
用顶点表示优化变量，用边表示误差项。
对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。
g2o默认使用四元数和平移向量表达位姿。

//头文件

#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_binary_edge.h>//g2o边（edge）头文件
#include <g2o/core/block_solver.h>//求解器头文件
#include <g2o/core/optimization_algorithm_levenberg.h>//列文伯格——马尔夸特算法头文件
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>//鲁棒核函数

// 选择优化方法

typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3> > SlamBlockSolver;//求解的向量是6*3的
// 选择一个线性方程求解器LinearSolver：这里选择的是CSparse，每个误差项优化变量维度为6，误差值维度为3；即６为pose的维度，３表示landmark的维度
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
// 初始化求解器
std::unique_ptr linearSolver ( new SlamLinearSolver());
linearSolver->setBlockOrdering( false );
//选择一个块求解器blockSolver：块求解器是包含线性求解器的存在，之所以是包含，是因为块求解器会构建好线性求解器所需要的矩阵块（也就是H和b）
std::unique_ptr blockSolver ( new SlamBlockSolver ( std::move(linearSolver)));
//选择一个迭代策略OptimizationAlgorithmL：这里选择的是LM
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(blockSolver));
//【关键】得到一个图模型globalOptimizer
g2o::SparseOptimizer globalOptimizer;
//设置求解器
globalOptimizer.setAlgorithm( solver );
// 不要输出调试信息
globalOptimizer.setVerbose( false );

// 向globalOptimizer增加第一个顶点
g2o::VertexSE3* v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
v->setFixed( true ); //第一个顶点固定，不用优化
globalOptimizer.addVertex( v );

//******帧间匹配程序，通过两张图像算出变换矩阵(略)

// 向图中增加顶点，顶点只需设定id即可
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );
globalOptimizer.addVertex(v);

// 向图中增加边
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );

// 每条边设定一个信息矩阵（协方差矩阵之逆），作为不确定性的度量：信息矩阵 Ω 是协方差矩阵的逆，是一个对称矩阵。它的每个元素可以看成我们对ei,ej这个误差项相关性的一个预计。最简单的是把Ω设成对角矩阵，对角阵元素的大小表明我们对此项误差的重视程度。例如：你觉得帧间匹配精度在0.1m，那么把信息矩阵设成100的对角阵即可。
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立；那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation( information );
// 边的估计即是pnp求解之结果
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);

// 调用optimizer.optimize( steps )优化所有边
cout<<“optimizing pose graph, vertices: “<<globalOptimizer.vertices().size()<<endl;
globalOptimizer.save(”./data/result_before.g2o”);
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定优化步数

摘录：https://www.cnblogs.com/gaoxiang12/p/5244828.html
做图优化的流程

1.选择你想要的图里的节点与边的类型，确定它们的参数化形式；
2.往图里加入实际的节点和边；
3.选择初值，开始迭代；
4.每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵；
5求解稀疏线性方程HkΔx=−bk，得到梯度方向；
6.继续用GN或LM进行迭代。如果迭代结束，返回优化值。
实际上，g2o能帮你做好第3-6步，你要做的只是前两步而已。

摘录：https://www.cnblogs.com/gaoxiang12/p/5304272.html
在g2o中选择优化方法一共需要三个步骤：

1.选择一个线性方程求解器，从 PCG（共轭梯度法）, CSparse（稀疏矩阵）, Choldmod（Cholesky分解对称正定阵）中选（不同的方法主要表现在最终的H矩阵构造不同。），实际则来自 g2o/solvers 文件夹中定义的东东。
2.选择一个 BlockSolver 。
3.选择一个迭代策略，从GN, LM, Doglog中选。

在这里插入图片描述

摘录：https://www.jianshu.com/p/e16ffb5b265d
SLAM使用g2o过程（详见slamEnd.cpp）

使用g2o来实现图优化还是比较容易的。它帮你把节点和边的类型都定义好了，基本上只需使用它内置的类型而不需自己重新定义。要构造一个图，要做以下几件事：

    定义一个SparseOptimizer.
    编写方式参见tutorial_slam2d的声明方式。你还要写明它使用的算法。通常是Gauss-Newton或LM算法。个人觉得后者更好一些。
    定义你要用到的边、节点的类型。
    由于我们是3D的slam，所以顶点取成了相机姿态：g2o::VertexSE3，而边则是连接两个VertexSE3的边：g2o::EdgeSE3。如果你想用别的类型的顶点（如2Dslam，路标点），你可以看看/usr/local/include/g2o/types/下的文件，基本上涵盖了各种slam的应用，应该能满足你的需求。
    例如我们实现一个3D SLAM。那么就要看它的g2o/types/slam3d下面的头文件。节点头文件都以vertex_开头，而边则以edge_开头。在我们上面的模型中，可以选择vertex_se3作为节点，edge_se3作为边。这两个类型的节点和边的数据都可以直接来自于Eigen::Isometry，即上面讲到过的变换矩阵T。
    编写一个帧间匹配程序，通过两张图像算出变换矩阵。这个用opencv, pcl都可以做。
    把你得到的关键帧作为节点，变换矩阵作为边，加入到optimizer中。
    同时设定节点的估计值（如果没有惯性测量就设成零）与边的约束（变换矩阵）。
    每条边还需设定一个信息矩阵（协方差矩阵之逆）作为不确定性的度量。
    例如你觉得帧间匹配精度在0.1m，那么把信息矩阵设成100的对角阵即可。
    在程序运行过程中不断作帧间检测，维护你的图。
    程序结束时调用optimizer.optimize( steps )进行优化。优化完毕后读取每个节点的估计值，此时就是优化后的机器人轨迹。
————————————————
版权声明：本文为优快云博主「思盖木木」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.youkuaiyun.com/qq_40313712/article/details/86500116