从零学习SLAM位姿（二）
觉SLAM位姿估计（总结）
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
转载链接：https://blog.csdn.net/u011178262/article/details/89685134
文章目录
- Features Based Method
- 2D-2D: Epipolar Geometry
- 3D-2D: PnP
- Bundle Adjustment
- 3D-3D: ICP
- 线性代数方式
- Optical Flow
- LK光流
- 分类
- 讨论
- Direct Method
- 参考文献
- g2o
相关代码：
- pose_estimation in cggos/slam_park_cg
Features Based Method
2D-2D: Epipolar Geometry
2D-2D 对极几何 主要涉及到基础矩阵、本质矩阵和单应性矩阵的求解，并从中恢复出旋转矩阵 RRR 和平移向量 ttt。
- 计算机视觉对极几何之FEH
同时，还要根据匹配的特征点和计算出的相对位姿进行三角化，恢复出 3D空间点。
- 计算机视觉对极几何之Triangulate(三角化)
在单目视觉SLAM中，以上过程主要用于SLAM的初始化：计算第二关键帧的相对位姿（假设第一帧位姿为 [I0][I \quad 0][I0]），并计算初始Map。
OpenCV 中相关函数：
- findFundamentalMat
- findEssentialMat
- findHomography
- recoverPose
- decomposeEssentialMat
- triangulatePoints
上文是特征，下文是位姿求取方法。
理解图优化，一步步带你看懂g2o框架
小白：师兄师兄，最近我在看SLAM的优化算法，有种方法叫“图优化”。
师兄：图优化的英文是 graph optimization 或者 graph-based optimization，你看，它的“图”其实是数据结构中的graph。而凸优化的英文是 convex optimization，这里的“凸”其实是凸函数的意思，所以单从英文就能区分开它们。
先了解g2o 框架
师兄：前面我们简单介绍了图优化，你也看到了它的神通广大，那如何编程实现呢？
小白：对啊，有没有现成的库啊，我还只是个“调包侠”。。
师兄：这个必须有啊！在SLAM领域，基于图优化的一个用的非常广泛的库就是g2o，它是General Graphic Optimization 的简称，是一个用来优化非线性误差函数的c++框架。这个库可以满足你调包侠的梦想~
师兄：我先说安装吧，其实g2o安装很简单，参考GitHub上官网：
https://github.com/RainerKuemmerle/g2o
按照步骤来安装就行了。需要注意的是安装之前确保电脑上已经安装好了第三方依赖。
小白：好的，这个看起来很好装。不过问题是，我看相关的代码，感觉很复杂啊，不知如何下手啊
师兄：别急，第一次接触g2o，确实有这种感觉，而且官网文档写的也比较“不通俗不易懂”，不过如果你能捋顺了它的框架，再去看代码，应该很快能够入手了
小白：是的，先对框架了然于胸才行，不然即使能凑合看懂别人代码，自己也不会写啊！
师兄：嗯嗯，其实g2o帮助我们实现了很多内部的算法，只是在进行构造的时候，需要遵循一些规则，在我看来这是可以接受的，毕竟一个程序不可能满足所有的要求，因此在以后g2o的使用中还是应该多看多记，这样才能更好的使用这个库。
小白：记住了。养成记笔记的好习惯，还要多练习。
师兄：好，那我们首先看一下下面这个图，是g2o的基本框架结构。如果你查资料的话，你会在很多地方都能看到。看图的时候要注意箭头类型
1、图的核心
小白：师兄，这个图该从哪里开始看？感觉好多东西。。
师兄：如果你想要知道这个图中哪个最重要，就去看看箭头源头在哪里
小白：我看看。。。好像是最左侧的SparseOptimizer？
师兄：对的，SparseOptimizer是整个图的核心，我们注意右上角的 is-a 实心箭头，这个SparseOptimizer它是一个Optimizable Graph，从而也是一个超图（HyperGraph）。
小白：我去，师兄，怎么突然冒出来这么多奇怪的术语，都啥意思啊？
师兄：这个你不需要一个个弄懂，不然可能黄花菜都凉了。你先暂时只需要了解一下它们的名字，有些以后用不到，有些以后用到了再回看。目前如果遇到重要的我会具体解释。
小白：好。那下一步看哪里？
2、顶点和边
师兄：我们先来看上面的结构吧。注意看 has-many 箭头，你看这个超图包含了许多顶点（HyperGraph::Vertex）和边（HyperGraph::Edge）。而这些顶点顶点继承自 Base Vertex，也就是OptimizableGraph::Vertex，而边可以继承自 BaseUnaryEdge（单边）, BaseBinaryEdge（双边）或BaseMultiEdge（多边），它们都叫做OptimizableGraph::Edge
小白：头有点晕了，师兄
师兄：哈哈，不用一个个记，现阶段了解这些就行。顶点和边在编程中很重要的，关于顶点和边的定义我们以后会详细说的。下面我们来看底部的结构。
小白：嗯嗯，知道啦！
3、配置SparseOptimizer的优化算法和求解器
师兄：你看下面，整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell's dogleg 三者中间选择一个（我们常用的是GN和LM）
小白：GN和LM就是我们以前讲过的非线性优化方法中常用的两种吧
师兄：是的，如果不了解的话具体看《从零开始学习「张氏相机标定法」（四）优化算法前传》《从零开始学习「张氏相机标定法」（五）优化算法正传》这两篇文章。
4、如何求解
师兄：那么如何求解呢？OptimizationWithHessian 内部包含一个求解器（Solver），这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix ，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod，具体定义后面会介绍
到此，就是上面图的一个简单理解。
一步步带你看懂g2o编程流程
小白：师兄，看完了我也不知道编程时具体怎么编呢！
师兄：我正好要说这个。首先这里需要说一下，我们梳理是从顶层到底层，但是编程实现时需要反过来，像建房子一样，从底层开始搭建框架一直到顶层。g2o的整个框架就是按照下图中我标的这个顺序来写的。
高博在十四讲中g2o求解曲线参数的例子来说明，源代码地址
https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp
为了方便理解，我重新加了注释。如下所示，
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // 每个误差项优化变量维度为3，误差值维度为1
// 第1步：创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense();
// 第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 i=0; isetId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
// 第6步：设置优化参数，开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100); 结合上面的流程图和代码。下面一步步解释具体步骤。
1、创建一个线性求解器LinearSolver
我们要求的增量方程的形式是：H△X=-b，通常情况下想到的方法就是直接求逆，也就是△X=-H.inv*b。看起来好像很简单，但这有个前提，就是H的维度较小，此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时，矩阵求逆变得很困难，求解问题也变得很复杂。
小白：那有什么办法吗？
师兄：办法肯定是有的。此时我们就需要一些特殊的方法对矩阵进行求逆，你看下图是GitHub上g2o相关部分的代码
如果你点进去看，可以分别查看每个方法的解释，如果不想挨个点进去看，看看下面我的总结就行了
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver2、创建BlockSolver。并用上面定义的线性求解器初始化。
BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化。它的定义在如下文件夹内：
g2o/g2o/core/block_solver.h
你点进去会发现 BlockSolver有两种定义方式
一种是指定的固定变量的solver，我们来看一下定义
using BlockSolverPL = BlockSolver< BlockSolverTraits >; 其中p代表pose的维度（注意一定是流形manifold下的最小表示），l表示landmark的维度
另一种是可变尺寸的solver，定义如下
using BlockSolverX = BlockSolverPL; 小白：为何会有可变尺寸的solver呢？
师兄：这是因为在某些应用场景，我们的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初始化
我们来看g2o/g2o/core/ 目录下，发现Solver的优化方法有三种：分别是高斯牛顿（GaussNewton）法，LM（Levenberg–Marquardt）法、Dogleg法，如下图所示，也和前面的图相匹配
小白：师兄，上图最后那个OptimizationAlgorithmWithHessian 是干嘛的？
师兄：你点进去 GN、 LM、 Doglet算法内部，会发现他们都继承自同一个类：OptimizationWithHessian，如下图所示，这也和我们最前面那个图是相符的
然后，我们点进去看 OptimizationAlgorithmWithHessian，发现它又继承自OptimizationAlgorithm，这也和前面的相符总之，在该阶段，我们可以选则三种方法：
g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg 4、创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。
创建稀疏优化器
g2o::SparseOptimizer optimizer;用前面定义好的求解器作为求解方法：
其中setVerbose是设置优化过程输出信息用的
SparseOptimizer::setVerbose(bool verbose)
5、定义图的顶点和边。并添加到SparseOptimizer中。
这部分比较复杂，我们下一次再介绍。
6、设置优化参数，开始执行优化。
设置SparseOptimizer的初始化、迭代次数、保存结果等。
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)
设置迭代次数，然后就开始执行图优化了。
SparseOptimizer::optimize(int iterations, bool online)
小白：终于搞明白g2o流程了！谢谢师兄！必须给你个「好看」啊！
注：以上内容部分参考了如下文章，感谢原作者：
https://www.jianshu.com/p/e16ffb5b265d
https://blog.csdn.net/heyijia0327/article/details/47686523
讨论
我们知道（不知道的话，去查一下十四讲）用g2o和ceres库都能用来进行BA优化，这两者在使用过程中有什么不同？