学习高翔一起做rgbd-slam中关于g20部分的学习一

第一步：LinearSolver：线性求解器

第二步：BlockSolver：块求解器

第三步：solver：总求解器

第四步：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是设置优化过程输出信息用的

第五步添加顶点和边

顶点：

一：先来看看上图中和vertex有关的第①个类： HyperGraph::Vertex。它在这个路径g2o/core/hyper_graph.h。 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。

二：图中第②个类，我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的，而OptimizableGraph的定义在g2o/core/optimizable_graph.h。我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph。

三：OptimizableGraph::Vertex 非常底层，具体使用时一般都会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中对应的第③个类：BaseVertex 路径：g2o/core/base_vertex.h

BaseVertex<D,T>

D：并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开.

T：就是顶点（状态变量）的类型.

自定义顶点一般需要考虑重写如下函数：

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以。

setToOriginImpl：顶点重置函数，设定被优化变量的原始值。

oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。

本代码不需要自己定义，用g2o本身的顶点类型就行

g2o本身内部定义了一些常用的顶点类型

VertexSE2 : public BaseVertex<3, SE2>  //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3>  //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>

// 第5步：定义图的顶点,并添加到SparseOptimizer中
g2o::VertexSE3 *v = new g2o::VertexSE3();//g2o本身内部定义了一些常用的顶点类型
v->setId( currIndex ); //setId(int) 定义节点编号
/*setEstimate(type) 函数来设定初始值估计为单位矩阵  Identity是VertexSE3中设置打默认值VertexSE3();virtual void setToOriginImpl() {_estimate = Isometry3::Identity();}
*/
v->setEstimate( Eigen::Isometry3d::Identity() );
v->setFixed( true ); //第一个顶点固定，不用优化
globalOptimizer.addVertex( v );//添加顶点到globalOptimizer

边：

BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。

一元边你可以理解为一条边只连接一个顶点。

两元边理解为一条边连接两个顶点，也就是我们常见的边啦。

多元边理解为一条边可以连接多个（3个以上）顶点

边和顶点的成员函数还是差别比较大的，边主要有以下几个重要的成员函数

virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();

read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian

如果不自定义边不用在意这些，用g2o提供的边就行

        // 边部分g2o::EdgeSE3* edge = new g2o::EdgeSE3();// 连接此边的两个顶点idedge->vertices() [0] = globalOptimizer.vertex( lastIndex );edge->vertices() [1] = globalOptimizer.vertex( currIndex );// 信息矩阵Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵information(0,0) = information(1,1) = information(2,2) = 100;information(3,3) = information(4,4) = information(5,5) = 100;// 也可以将角度设大一些，表示对角度的估计更加准确edge->setInformation( information );// 信息矩阵：协方差矩阵之逆// 边的估计即是pnp求解之结果 观测数值edge->setMeasurement( T );// 将此边加入图中globalOptimizer.addEdge(edge);

这个代码往图中加入：边连接两个顶点的ID,信息矩阵(协方差矩阵之逆 )，观测数值(T的值)

整体slamEnd代码如下：

#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;//g2o的头文件
#include "slamBase.h"
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>FRAME readFrame(int index,ParameterReader& pd);double normofTransform(cv::Mat rvec,cv::Mat tvec);int main(int argc,char** argv)
{ParameterReader pd;int startIndex = atoi(pd.getData("start_index").c_str());int endIndex   = atoi(pd.getData("end_index").c_str());cout<<"Initializing..."<<endl;int currIndex = startIndex;// 当前索引为currIndexFRAME lastFrame = readFrame(currIndex,pd);//获取这个index的帧的数据string detector = pd.getData("detector");string descriptor = pd.getData("descriptor");CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();computeKeyPointsAndDesp(lastFrame,detector,descriptor);PointCloud::Ptr cloud = image2PointCloud(lastFrame.rgb,lastFrame.depth,camera);pcl::visualization::CloudViewer viewer("viewer");// 是否显示点云bool visualize = pd.getData("visualize_pointcloud") == string("yes");int min_inliers = atoi(pd.getData("min_inliers").c_str());double max_norm = atof(pd.getData("max_norm").c_str());typedef g2o::BlockSolver_6_3 SlamBlockSolver;//先定义一个BlockSolver;表示pose是6维，观测点是3维。用于3D SLAM中的BA//依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolvertypedef g2o::LinearSolverEigen< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;//在把LinearSolverEigen定义好// 第1步：创建一个线性求解器LinearSolverSlamLinearSolver* linearSolver = new SlamLinearSolver();linearSolver->setBlockOrdering( false );// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化SlamBlockSolver* blockSolver = new SlamBlockSolver( unique_ptr<SlamLinearSolver>(linearSolver));// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<SlamBlockSolver>(blockSolver) );// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）g2o::SparseOptimizer globalOptimizer;  // 最后用的就是这个东东 图模型 创建稀疏优化器globalOptimizer.setAlgorithm( solver );// 设置求解器 用前面定义好的求解器作为求解方法globalOptimizer.setVerbose( false );// 关闭调试输出 其中setVerbose是设置优化过程输出信息用的// 第5步：定义图的顶点和边。并添加到SparseOptimizer中g2o::VertexSE3 *v = new g2o::VertexSE3();//g2o本身内部定义了一些常用的顶点类型v->setId( currIndex ); //setId(int) 定义节点编号v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵 setEstimate(type) 函数来设定初始值v->setFixed( true ); //第一个顶点固定，不用优化globalOptimizer.addVertex( v );//添加顶点到globalOptimizerint lastIndex = currIndex;//循环for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ){cout<<"Reading files "<<currIndex<<endl;FRAME currFrame = readFrame( currIndex,pd ); // 读取currFramecomputeKeyPointsAndDesp( currFrame, detector, descriptor );// 比较currFrame 和 lastFrameRESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );if ( result.inliers < min_inliers ) //inliers不够，放弃该帧continue;// 计算运动范围是否太大double norm = normofTransform(result.rvec, result.tvec);cout<<"norm = "<<norm<<endl;if ( norm >= max_norm )continue;Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );cout<<"T="<<T.matrix()<<endl;// 向g2o中增加这个顶点与上一帧联系的边// 顶点部分// 顶点只需设定id即可//g2o::VertexSE3 *v = new g2o::VertexSE3();v->setId( currIndex );v->setEstimate( Eigen::Isometry3d::Identity() );//估计为单位矩阵 setEstimate(type) 函数来设定初始值globalOptimizer.addVertex(v);// 边部分g2o::EdgeSE3* edge = new g2o::EdgeSE3();// 连接此边的两个顶点idedge->vertices() [0] = globalOptimizer.vertex( lastIndex );edge->vertices() [1] = globalOptimizer.vertex( currIndex );// 信息矩阵Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵information(0,0) = information(1,1) = information(2,2) = 100;information(3,3) = information(4,4) = information(5,5) = 100;// 也可以将角度设大一些，表示对角度的估计更加准确edge->setInformation( information );// 信息矩阵：协方差矩阵之逆// 边的估计即是pnp求解之结果 观测数值edge->setMeasurement( T );// 将此边加入图中globalOptimizer.addEdge(edge);lastFrame = currFrame;lastIndex = currIndex;// cloud = joinPointCloud( cloud, currFrame, T, camera );}//pcl::io::savePCDFile( "../data/result.pcd", *cloud );cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;// 第6步：设置优化参数，开始执行优化globalOptimizer.initializeOptimization();globalOptimizer.optimize( 100 ); //可以指定优化步数globalOptimizer.save( "/home/stf/MySlam_two/r.g2o" );cout<<"Optimization done."<<endl;globalOptimizer.clear();return 0;return 0;
}
FRAME readFrame(int index,ParameterReader& pd)
{FRAME f;string rgbDir    =   pd.getData("rgb_dir");string depthDir  =   pd.getData("depth_dir");string rgbExt    =   pd.getData("rgb_extension");string depthExt  =   pd.getData("depth_extension");stringstream ss;ss<<rgbDir<<index<<rgbExt;string filename;ss>>filename;cout<<filename;f.rgb = cv::imread(filename);ss.clear();filename.clear();ss<<depthDir<<index<<depthExt;ss>>filename;f.depth = cv::imread(filename,-1);f.frameID = index;return f;}
double normofTransform(cv::Mat rvec,cv::Mat tvec)
{return fabs(min(cv::norm(rvec),2*M_PI-cv::norm(rvec))) + fabs(cv::norm(tvec));
}

附上参考资料 g2o整体框架顶点理解边的理解