SLAM本质剖析-G2O
0. 前言
在了解SLAM的原理、流程后,个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免,尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。作者分别从Ceres和Eigen两个函数进行了深入的解析,这一篇文章主要对G2O函数库进行详细的阐述,来方便各位后续的开发。
1.G2O示例
相较于Ceres而言,G2O函数库相对较为复杂,但是适用面更加广,可以解决较为复杂的重定位问题。Ceres库向通用的最小二乘问题的求解,定义优化问题,设置一些选项,可通过Ceres求解。而图优化,是把优化问题表现成图的一种方式,这里的图是图论意义上的图。一个图由若干个顶点,以及连着这些顶点的边组成。在这里,我们用顶点表示优化变量,而用边表示误差项。
为了使用g2o,首先要将曲线拟合问题抽象成图优化。这个过程中,只要记住节点为优化变量,边为误差项即可。曲线拟合的图优化问题可以画成以下形式:
G2O在数学上主要分为四个求解步骤:
在程序中的反应为:
- 创建一个线性求解器LinearSolver。
- 创建BlockSolver,并用上面定义的线性求解器初始化。
- 创建总求解器solver,并从GN/LM/DogLeg 中选一个作为迭代策略,再用上述块求解器BlockSolver初始化。
- 创建图优化的核心:稀疏优化器(SparseOptimizer)。
- 定义图的顶点和边,并添加到SparseOptimizer中。
- 设置优化参数,开始执行优化。
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std; // 曲线模型的顶点,模板参数:优化变量维度和数据类型(此处为自定义定点,具体可以参考下方的点定义)
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 字节对齐virtual void setToOriginImpl() // 重置,设定被优化变量的原始值 {_estimate << 0,0,0;}virtual void oplusImpl( const double* update ) // 更新{_estimate += Eigen::Vector3d(update);//update强制类型转换为Vector3d}// 存盘和读盘:留空virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
};// 误差模型 模板参数:观测值维度,类型,连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差void computeError(){const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);const Eigen::Vector3d abc = v->estimate();_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;}virtual bool read( istream& in ) {}virtual bool write( ostream& out ) const {}
public:double _x; // x 值, y 值为 _measurement
};int main( int argc, char** argv )
{double a=1.0, b=2.0, c=1.0; // 真实参数值int N=100; // 数据点double w_sigma=1.0; // 噪声Sigma值cv::RNG rng; // OpenCV随机数产生器double abc[3] = {0,0,0}; // abc参数的估计值vector<double> x_data, y_data; // 数据cout<<"generating data: "<<endl;for ( int i=0; i<N; i++ ){double x = i/100.0;x_data.push_back ( x );y_data.push_back (exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma ));cout<<x_data[i]<<" "<<y_data[i]<<endl;}// 构建图优化,先设定g2otypedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3,误差值维度为1Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器。第一步:创建一个线性求解器LinearSolver。Block* solver_ptr = new Block( linearSolver ); // 矩阵块求解器。第二步:创建BlockSolver,并用上面定义的线性求解器初始化。// 梯度下降方法,从GN, LM, DogLeg 中选g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );//第三步:创建BlockSolver,并用上面定义的线性求解器初始化。// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );g2o::SparseOptimizer optimizer; // 图模型。第四步:创建图优化的核心:稀疏优化器(SparseOptimizer)。optimizer.setAlgorithm( solver ); // 设置求解器optimizer.setVerbose( true ); // 打开调试输出// 往图中增加顶点。第五步:定义图的顶点和边HyperGraph,并添加到SparseOptimizer中。CurveFittingVertex* v = new CurveFittingVertex();v->setEstimate( Eigen::Vector3d(0,0,0) );v->setId(0);optimizer.addVertex( v );// 往图中增加边for ( int i=0; i<N; i++ ){CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );edge->setId(i);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 );}// 执行优化。第六步:设置优化参数,开始执行优化。cout<<"start optimization"<<endl;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();optimizer.initializeOptimization();optimizer.optimize(100);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;// 输出优化值Eigen::Vector3d abc_estimate = v->estimate();cout<<"estimated model: "<<abc_estimate.transpose()<<endl;return 0;
}
2. G2O常见函数总结
如下图所示,这个图反应了上述的前五个步骤
对这个结构框图做一个简单介绍(注意图中三种箭头的含义(右上角注解)):
(1)$\color{#4285f4}{图的核心:整个g2o框架可以分为上下两部分,两部分中间的连接点:SparseOpyimizer 就是整个g2o的核心部分。
(2)往上看,SparseOpyimizer其实是一个Optimizable Graph,从而也是一个超图(HyperGraph)。
(3)顶点和边:\color{#4285f4}{顶点和边:}顶点和边:超图有很多顶点和边。顶点继承自 Base Vertex,也即OptimizableGraph::Vertex;而边可以继承自 BaseUnaryEdge(单边), BaseBinaryEdge(双边)或BaseMultiEdge(多边),它们都叫做OptimizableGraph::Edge
。
(4)配置SparseOptimizer的优化算法和求解器:\color{#4285f4}{配置SparseOptimizer的优化算法和求解器:}配置SparseOptimizer的优化算法和求解器:往下看,SparseOptimizer包含一个优化算法部分OptimizationAlgorithm,它是通过OptimizationWithHessian 来实现的。其中迭代策略可以从**Gauss-Newton(高斯牛顿法,简称GN)、 Levernberg-Marquardt(简称LM法)、Powell’s dogleg **三者中间选择一个(常用的是GN和LM)。
(5)如何求解:\color{#4285f4}{如何求解:}如何求解:对优化算法部分进行求解的时求解器solver,它实际由BlockSolver 组成。BlockSolver由两部分组成:一个是SparseBlockMatrix,它由于求解稀疏矩阵(雅克比和海塞);另一个部分是LinearSolver,它用来求解线性方程 得到待求增量,因此这一部分是非常重要的,它可以从PCG/CSparse/Choldmod
选择求解方法。
2.1 创建一个线性求解器LinearSolver
我们要求的增量方程的形式是:H△X=-b,通常情况下想到的方法就是直接求逆,也就是△X=-H.inv*b。看起来好像很简单,但这有个前提,就是H的维度较小,此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时,矩阵求逆变得很困难,求解问题也变得很复杂。和Ceres类似,G2O需要一些特殊的方法对矩阵进行求逆。
…详情请参照古月居
SLAM本质剖析-G2O相关推荐
- SLAM本质剖析番外-李群李代数的微分和导数
0. 简介 这几个月,博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享,之前也分享了李群李代数关注核心一文,从现象中解释了李群和李代数表达的含义.但是这还不够,所以这次作者作为SLAM本 ...
- SLAM本质剖析-Boost
0. 简介 在深入剖析了Ceres.Eigen.Sophus.G2O后,以V-SLAM为代表的计算方式基本已经全部讲完.就L-SLAM而言,本系列也讲述了PCL.Open3D.与GTSAM点云计算部分 ...
- SLAM本质剖析-Open3D
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨lovely_yoshino 来源丨古月居 0. 前言 在深入剖析了Ceres.Eigen.So ...
- SLAM本质剖析-Boost之Geometry函数大全(二)
4. 点云处理 4.1 add_point 将一个点添加到另一个点 4.2 add_value 将相同的值添加到点的每个坐标 4.3 assign_point 用另一个点指定一个点 4.4 assig ...
- SLAM图优化g2o
SLAM图优化g2o 图优化g2o框架 图优化的英文是 graph optimization 或者 graph-based optimization, "图"其实是数据结构中的gr ...
- 激光SLAM深度剖析
激光SLAM深度剖析系列文章索引 本系列文章,是对激光SLAM的深度解读,一作为学习用,二作为分享用. 详细的解释.直观的图表.丰富的注释.简化的测试代码. 作者:公众号:小白学移动机器人 之前写过的 ...
- GTSAM 学习(一)
GTSAM(Georgia Tech Smoothing and Mapping)是基于因子图的C++库,它可以解决slam和sfm的问题,当然它也可以解决简单或者更加复杂的估计问题. 主要由以下三个 ...
- 基于ROS2开发的点云栅格化
0. 简介 最近在收到了很多读者的消息后,我觉得有必要开这个坑,来给大家阐述下如何对激光雷达点云以及图像点云去做栅格化以及体素化的操作.这部分需要各位读者拥有PCL,octomap,ROS2,C++的 ...
- Ceres 常用的优化手段
0. 简介 我们知道,在SLAM中,最主要也最关键的就是如何对我们设计出来的非线性问题进行求解,并计算出最优解.而在这个过程中我们用的最多的就是Ceres,之前作者在<SLAM本质剖析-Cere ...
最新文章
- android相关使用工具
- MxNet教程:使用一台机器训练1400万张图片
- linux服务器定时关机重启,Ubuntu Server 10.10 每天定时开关机linux服务器应用 -电脑资料...
- poj 2387 Til the Cows Come Home dijkstra
- Codeforces 817F MEX Queries
- 卡巴斯基称新型黑客雇佣组织正在攻击欧洲律所
- /dev/shm目录下产生大量的ora_$ORACLE_SID的二进制文件
- 七年级计算机学期教学要求,七年级信息技术教学工作总结
- 深聊性能测试,从入门到放弃之: Windows系统性能监控(三)任务管理器介绍及使用。
- 三星4k3d电视测试软件,说出来你可能不信:3D、4K、曲面电视慎重购买
- U8G2库移植到STM32平台上
- 用unbound搭建简单的DNS服务器
- 计算机视觉方向面试题总结(二):图像分割篇
- Gemini Blueprint参考文档 第9章 服务注册表
- 数据结构-栈--表达式
- Twitter 推荐算法开源,马斯克被“特殊照顾”
- 2020高压电工考试题库及高压电工模拟考试题
- 新浪博客个人域名升级奖励
- SSL 2382 k好数(数位dp)
- 基于AT89C52单片机的简易电子琴设计与仿真