wordpress 网站建设,网页制作专业分析,论坛门户网站建设运营费用,昌吉州住房和城乡建设局网站从零开始一起学习SLAM | 理解图优化#xff0c;一步步带你看懂g2o代码
SLAM的后端一般分为两种处理方法#xff0c;一种是以扩展卡尔曼滤波#xff08;EKF#xff09;为代表的滤波方法#xff0c;一种是以图优化为代表的非线性优化方法。不过#xff0c;目前SLAM研究的主…从零开始一起学习SLAM | 理解图优化一步步带你看懂g2o代码
SLAM的后端一般分为两种处理方法一种是以扩展卡尔曼滤波EKF为代表的滤波方法一种是以图优化为代表的非线性优化方法。不过目前SLAM研究的主流热点几乎都是基于图优化的。
顺便总结下滤波方法的优缺点
优点在当时计算资源受限、待估计量比较简单的情况下EKF为代表的滤波方法比较有效经常用在激光SLAM中。
缺点它的一个大缺点就是存储量和状态量是平方增长关系因为存储的是协方差矩阵因此不适合大型场景。而现在基于视觉的SLAM方案路标点特征点数据很大滤波方法根本吃不消所以此时滤波的方法效率非常低。
在SLAM里图优化一般分解为两个任务
1、构建图。机器人位姿作为顶点位姿间关系作为边。
2、优化图。调整机器人的位姿顶点来尽量满足边的约束使得误差最小。
g2o安装很简单参考GitHub上官网
https://github.com/RainerKuemmerle/g2o 1.顶点和边
注意看 has-many 箭头你看这个超图包含了许多顶点HyperGraph::Vertex和边HyperGraph::Edge。而这些顶点顶点继承自 Base Vertex也就是OptimizableGraph::Vertex而边可以继承自 BaseUnaryEdge单边, BaseBinaryEdge双边或BaseMultiEdge多边它们都叫做OptimizableGraph::Edge
2.配置SparseOptimizer的优化算法和求解器
整个图的核心SparseOptimizer 包含一个优化算法OptimizationAlgorithm的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton高斯牛顿法简称GN, Levernberg-Marquardt简称LM法, Powells dogleg 三者中间选择一个我们常用的是GN和LM
3.如何求解
OptimizationWithHessian 内部包含一个求解器Solver这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分一个是SparseBlockMatrix 用于计算稀疏的雅可比和Hessian矩阵一个是线性方程的求解器LinearSolver它用于计算迭代过程中最关键的一步HΔx−bLinearSolver有几种方法可以选择PCG, CSparse, Choldmod具体定义后面会介绍 高博在十四讲中g2o求解曲线参数的例子来说明源代码地址
https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp
typedef g2o::BlockSolver g2o::BlockSolverTraits3,1 Block; // 每个误差项优化变量维度为3误差值维度为1// 第1步创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver new g2o::LinearSolverDenseBlock::PoseMatrixType(); // 第2步创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr new Block( linearSolver ); // 第3步创建总求解器solver。并从GN, LM, DogLeg 中选一个再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// 第4步创建终极大boss 稀疏优化器SparseOptimizer
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出// 第5步定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v new CurveFittingVertex(); //往图中增加顶点
v-setEstimate( Eigen::Vector3d(0,0,0) );
v-setId(0);
optimizer.addVertex( v );
for ( int i0; iN; i ) // 往图中增加边
{CurveFittingEdge* edge new CurveFittingEdge( x_data[i] );edge-setId(i);edge-setVertex( 0, v ); // 设置连接的顶点edge-setMeasurement( y_data[i] ); // 观测数值edge-setInformation( Eigen::Matrixdouble,1,1::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵协方差矩阵之逆optimizer.addEdge( edge );//设置迭代次数
}// 第6步设置优化参数开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); 1.线性求解器
LinearSolverCholmod 使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG 使用preconditioned conjugate gradient 法继承自LinearSolver
LinearSolverDense 使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen 依赖项只有eigen使用eigen中sparse Cholesky 求解因此编译好后可以方便的在其他地方使用性能和CSparse差不多。继承自LinearSolver
2.创建BlockSolver。并用上面定义的线性求解器初始化。
BlockSolver 内部包含 LinearSolver用上面我们定义的线性求解器LinearSolver来初始化。
你点进去会发现 BlockSolver有两种定义方式一种是指定的固定变量的solver我们来看一下定义
using BlockSolverPL BlockSolver BlockSolverTraitsp, l ;
其中p代表pose的维度注意一定是流形manifold下的最小表示l表示landmark的维度
另一种是可变尺寸的solver定义如下
using BlockSolverX BlockSolverPLEigen::Dynamic, Eigen::Dynamic; 这是因为在某些应用场景我们的Pose和Landmark在程序开始时并不能确定那么此时这个块状求解器就没办法固定变量此时使用这个可变尺寸的solver所有的参数都在中间过程中被确定另外你看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初始化
你点进去 GN、 LM、 Doglet算法内部会发现他们都继承自同一个类OptimizationWithHessian如下图所示这也和我们最前面那个图是相符的。然后我们点进去看 OptimizationAlgorithmWithHessian发现它又继承自OptimizationAlgorithm这也和前面的相符总之在该阶段我们可以选则三种方法
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)
https://www.jianshu.com/p/e16ffb5b265d
https://blog.csdn.net/heyijia0327/article/details/47686523
