RGB-D SLAM学习总结(6)

第六讲 加入图优化

---

上一讲的视觉里程计结果明显可以看到当相机重新转回来的时候，出现了明显的偏差。图优化就是为了改善每一次的匹配，不仅仅考虑两帧的信息，而要把所有整的信息都考虑进来，成为一个全slam问题（full slam）

---

添加g2o的依赖，在CMakeLists.txt里加入：

# 添加g2o的依赖
# 因为g2o不是常用库，要添加它的findg2o.cmake文件
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
SET(G2O_ROOT "/usr/local/include/g2o")
FIND_PACKAGE(G2O)
# CSparse
FIND_PACKAGE(CSparse)
INCLUDE_DIRECTORIES(${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR})

注意：${PROJECT_SOURCE_DIR}是这个项目的根目录，也就是在${PROJECT_SOURCE_DIR}/cmake_modules文件夹里去找findg2o.cmake文件，所以我们要把g2o源码里的这个文件夹拿过来（里面有findg2o.cmake文件）：

否则就写成：

LIST(APPEND CMAKE_MODULE_PATH /home/lzy/g2o/cmake_modules)

加入图优化的代码：https://www.cnblogs.com/gaoxiang12/p/4739934.html

相比于第五讲的VO代码，在每一帧读入处理时，将其加入一个图中，顶点代表相机的位姿，也就是需要优化的变量，边表示两帧之间位姿的T转换矩阵。最后对这个图进行一次全局优化。

---

图优化原理可以详看：https://www.cnblogs.com/gaoxiang12/p/5244828.html

图优化库的使用可以详看：https://www.cnblogs.com/gaoxiang12/p/5304272.html

在g2o中选择优化方法一共需要三个步骤：

1. 选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选，实际则来自 g2o/solvers 文件夹中定义的东东。
2. 选择一个 BlockSolver 。
3. 选择一个迭代策略，从GN, LM, Doglog中选。

下面是代码中关于g2o初始化的部分：

    // 选择优化方法
    typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
    typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 

    // 初始化求解器
    SlamLinearSolver* linearSolver = new SlamLinearSolver();
    linearSolver->setBlockOrdering( false );
    SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );

    g2o::SparseOptimizer globalOptimizer;  // 最后用的就是这个东东
    globalOptimizer.setAlgorithm( solver ); 

可以看出：

1. 选用CSparse作为线性方程求解器。
2. 6*3的参数，pose的维度为6，关键点landmark的维度为3。
3. 迭代策略为LM。