目录
第四步:SparseOptimizer:稀疏优化器 最终要的就是这玩意
g2o:图优化中用的一个库;
在SLAM里,图优化一般分解为两个任务:
1、构建图:机器人位姿作为顶点,位姿间关系作为边。
2、优化图。调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小。
下面看一张整体图:
只需要按照步骤依次构建就好,下面解释一下每个名称的含义:
第一步:LinearSolver:线性求解器
LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver
LinearSolverDense :使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver
//先定义一个BlockSolver;表示pose是6维,观测点是3维。用于3D SLAM中的BA
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
//依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver
typedef g2o::LinearSolverEigen< SlamBlockSolver::PoseMatrixType SlamLinearSolver;//在把LinearSolverEigen定义好
// 第1步:创建一个线性求解器LinearSolver
SlamLinearSolver* linearSolver = new SlamLinearSolver();
第二步:BlockSolver:块求解器
BlockSolver 内部包含 LinearSolver,用上面我们定义的线性求解器LinearSolver来初始化
BlockSolver_6_3 :表示pose 是6维,观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3:在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2:表示pose 是3维,观测点是2维
// 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
SlamBlockSolver* blockSolver = new SlamBlockSolver( unique_ptr<SlamLinearSolver>(linearSolver));
第三步:solver:总求解器
并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
// 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<SlamBlockSolver>(blockSolver) );
第四步:SparseOptimizer:稀疏优化器 最终要的就是这玩意
// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 图模型 创建稀疏优化器
globalOptimizer.setAlgorithm( solver );// 设置求解器 用前面定义好的求解器作为求解方法
globalOptimizer.setVerbose( false );// 关闭调试输出 其中setVerbose是设置优化过程输出信息用的