G2O使用说明（SLAM中）

最新推荐文章于 2024-09-27 15:17:34 发布

月夕花晨TS

最新推荐文章于 2024-09-27 15:17:34 发布

阅读量1.6k

点赞数 4

文章标签：自动驾驶 c++ 算法

本文链接：https://blog.csdn.net/qq_45369294/article/details/119853279

版权

G2O使用说明（SLAM）

范例：在使用G2O要明确目标函数是什么，优化变量是几维，误差项是几维。举例说明：现在有很多个点符合 $y=exp(ax^{2}+bx+c)+w$ ,其中w为高斯噪声。那么目标函数就是 $e(a,b,c)=y-exp(ax^{2}+bx+c)$ 使得它最小(最小二乘法的原理)。优化变量就是a、b、c是三维的，误差项是e(a,b,c)的值，显然它是一个标量，它是一维的。

还要明白G2O是基于图优化进行设计的（节点与边的关系），将上述问题进行图展示：
在这里插入图片描述

其中：
节点：优化变量
边：节点与节点之间的关系
显然这个例子是一个一元边，这里边没有解释的太清楚，在后面的SLAM的例子中在进行重新解释。

1.初始G2O

1. 构建图优化

// 每个优化变量维度为3，误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block

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

std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>() );

3. 创建BlockSolver。并用上面定义的线性求解器初始化

std::unique_ptr<Block> solver_ptr (new Block( std::move(linearSolver) ));

4. 创建总求解器solver。并从GN, LM, DogLeg 中选一个迭代策略，再用上述块求解器BlockSolver初始化

//L-M迭代方法
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));

//G-N迭代方法
//g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr));

//DogLeg方法
//g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr));

5. 创建终极大boss 稀疏优化器（SparseOptimizer），并用已定义求解器作为求解方法。

g2o::SparseOptimizer optimizer;     // 图模型
optimizer.setAlgorithm( solver );   // 设置求解器
optimizer.setVerbose( true );       // 打开调试输出

6. 添加顶点
顶点可以继承一个模板类BaseVertex，模板参数为顶点的最小参数个数和顶点的数据类型。需要我们实现的是重写操作，重写 virtual void oplusImpl(const double *update) --节点的更新函数。估计值存储在变量_estimate中；函数void setToOriginImpl() 将节点的值进行重置。

重写顶点的类函数

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // 用到了new方法,指针对齐
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    } 
    virtual void oplusImpl( const double* update ) // 顶点的更新函数
    {    
        //这步进行的就是Xk+1=Xk+Δxk
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    //其中read，write函数可以不进行覆写，仅仅声明一下就可以了。
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

调用顶点的类函数

CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );  //设定初始值
v->setId(0); //定义节点编号
optimizer.addVertex( v );  //优化器中加入顶点

7. 添加边
单元边（BaseUnaryEdge<D,E,VertexXi>，二元（BaseBinaryEdge<D,E,VertexXi,VertexXj>，多元边（BaseMultiEdge<D,E>），元可以理解为顶点。边也是继承一个模板类，模板参数为误差向量e的维度，观测z的数据类型和边连接的顶点类型。这里需要注意的是，几个顶点就是几元边。边也就是上文中的目标函数 $e_{ij}$ ，我们需要为边定义误差函数void computeError()，定义雅克比 $J_{ij}$ void linearizeOplus() ,是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的 $J_{acobian}$ ，给g2o提供解析的导数。
重写边的类函数

// 误差模型 模板参数：观测值维度，边的类型，连接节点的类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        //节点存储在_vertices[],
        /*单元边，_vertices[]的大小为1，存储顺序和调用setVertex(int,vertex) 与设定的int有关（0）*/
        /*二元边，_vertices[]的大小为2，存储顺序和调用setVertex(int,vertex) 与设定的int有关（0或1）*/
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        //观测值存储在_measurement 中，误差存储在_error 中，
        //定义目标函数（测量值-观测值），这里测量值为y，观测值为exp(ax^2+bx+c)
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

调用边的类函数

 for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);                         //设置边的ID号
        //这个0和上面的_vertices[]一致，并且类型得相同
        edge->setVertex( 0, v );                // 设置连接的顶点
        //传入的就是上面的_measurement
        edge->setMeasurement( y_data[i] );      // 观测数值
        // 信息矩阵：协方差矩阵之逆
        Eigen::Matrix<double,1,1> temp=
        Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma);
        edge->setInformation(temp); 
        optimizer.addEdge( edge );           //优化器中加入顶点
    }

8. 执行优化并输出

//SparseOptimizer的初始化
optimizer.initializeOptimization();  
//SparseOptimizer的迭代次数
optimizer.optimize(100);
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();

到这里G2O基本操作已经介绍完毕了，在SLAM中还有一些知识上面没有涉及的，但整体框架是差不多的。

2.SLAM中的G2O的一些说明

1. g2o提供的顶点vertex

class VertexSE3Expmap:public BaseVertex<6,g2o::SE3Quat>

继承于 BaseVertex 这个模板类
需要设置的模板参数：
参数6：SE3Quat类型为六维，三维旋转，三维平移
参数SE3Quat：g2o内部的SE(3),必须使用它，不能使用Sophus。该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数。
该顶点需要设置的参数

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
//【1】 设置待优化位姿（这是粗略位姿）
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
//【2】 设置Id号
vSE3->setId(pKFi->mnId);   //int类型
//【3】 设置是否固定， 第一帧固定
vSE3->setFixed(pKFi->mnId==0);

2. g2o提供的空间点的位置

class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>

该顶点需要设置的参数

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
//【1】 设置待优化空间点3D位置XYZ
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
//【2】 设置Id号
vPoint->setId(id);
//【3】 是否边缘化（以便稀疏化求解）
vPoint->setMarginalized(true);

3. g2o提供的边edge

Point-Pose 二元边即要优化MapPoints的位置，又要优化相机的位姿

class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>

继承于 BaseBinaryEdge 这个二元边模板类
需要设置的模板参数：
参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度
参数Vector ：观测值类型， piexl.x， piexl.y
参数 VertexSBAPointXYZ ：第一个顶点类型
参数 VertexSE3Expmap ：第二个顶点类型
该边需要设置的参数：

//【1】 设置第一个顶点， 注意该顶点类型与模板参数第一个顶点类型对应
 e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
 //【2】 设置第二个顶点
 e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
 //【3】 设置观测值， 类型与模板参数对应
 e->setMeasurement(obs);
 const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
 //【4】 设置信息矩阵， 协方差
 e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
 //【5】 设置鲁棒核函数
 g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
 e->setRobustKernel(rk);
 rk->setDelta(thHuberMono);
 //【6】 设置相机内参
 e->fx = pKFi->fx;
 e->fy = pKFi->fy;
 e->cx = pKFi->cx;
 e->cy = pKFi->cy;

Pose 一元边 (SE3)
仅优化相机位姿，为了构造出投影方程，需要按下面的方式把MapPoints的位置作为常量加入

class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>

该边需要设置的参数：

 g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
 // 注意这里只设置一个顶点， 其它一样
 e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
 e->setMeasurement(obs);
 const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
 e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
 g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
 e->setRobustKernel(rk);
 rk->setDelta(deltaMono); /** @attention 设置阈值， 卡方自由度为2， 内点概率95%对应的临界值*/
 e->fx = pFrame->fx;
 e->fy = pFrame->fy;
 e->cx = pFrame->cx;
 e->cy = pFrame->cy;
 /** @attention 需要在这里设置<不做优化>的MapPoints的位置*/
 cv::Mat Xw = pMP->GetWorldPos();
 e->Xw[0] = Xw.at<float>(0);
 e->Xw[1] = Xw.at<float>(1);
 e->Xw[2] = Xw.at<float>(2);

Pose-Pose二元边(SE3-SE3)
优化变量是相机两个关键帧位姿，约束来自对这两个关键帧位姿变换的测量（里程计、 IMU等）

class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>

需要设置的参数如下：

 Se2 measure_se2 = pMsrOdo->se2;
 // 【1】 里程计测量的协方差
 g2o::Matrix3D covariance = toEigenMatrixXd(pMsrOdo->info).inverse();
 // 【2】 将里程计测量转换成g2o::SE3Quat类型
 Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
 g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure_se2.x, measure_se2.y,measure_se3.z);
 // 【3】 将`里程计测量协方差`转换为`相机坐标系下协方差`
 // 注意： g2o::SE3Quat是旋转在前， 平移在后
 g2o::Matrix6d covariance_6d = g2o::Matrix6d::Identity();
 covariance_6d(0,0) = covariance(2,2);
 covariance_6d(0,4) = covariance(2,0); covariance_6d(0,5) = covariance(2,1);
 covariance_6d(4,0) = covariance(0,2); covariance_6d(5,0) = covariance(1,2);
 covariance_6d(3,3) = covariance(0,0);
 covariance_6d(4,4) = covariance(1,1);
 covariance_6d(1,1) = 0.00001;
 covariance_6d(2,2) = 0.01;
 covariance_6d(5,5) = 0.0001;
 g2o::Matrix6d Info = g2o::Matrix6d::Identity();
 Info = covariance_6d.inverse();
 // 【4】 构造边
 g2o::EdgeOnlineCalibration* e = new g2o::EdgeOnlineCalibration;
 e->vertices()[0] = optimizer.vertex(id0);
 e->vertices()[1] = optimizer.vertex(id1);
 e->setMeasurement(relativePose_SE3Quat);
 e->setInformation(Info);
 optimizer.addEdge(e);

4. g2o提供的相机内参的优化

g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
); //后面的0表示camera的第几个参数
// 设置Id号为0即可
camera->setId ( 0 );
optimizer.addParameter ( camera );

如果不想优化相机内参，则内参按照第二步中二元边中的程序demo中设置

5. 检测outliner
优化完成后，对每一条边都进行检查，剔除误差较大的边（认为是错误的边），并设置 setLevel 为0，即下次不再对该边进行优化。

optimizer.optimize ( 100 );
// 优化完成后， 进行Edge的检查
for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
{
    g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
    MapPoint* pMP = vpMapPointEdgeMono[i];
    if(pMP->isBad())
    continue;
    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
    // 第二个判断条件， 用于检查构成该边的MapPoint在该相机坐标系下的深度是否为正？
    if(e->chi2()>5.991 || !e->isDepthPositive())
    {
       e->setLevel(1);// 不优化
    } /
    / 因为剔除了错误的边， 所以下次优化不再使用核函数
    e->setRobustKernel(0);
}

3.SLAM中的一些G2O例子

1. PnP例子(3D-2D)
目标函数：$ e(ε)=u-\frac{1}{s}Kexp(ε^{*})P)$ ，其中u是像素坐标，表示的是第二幅图的特征点，该特征点已经与第一幅图匹配好了，同时u也是测量数据。 $exp(ε^{*})$ 表示的是相机的位姿，用李代数表示，因为在李群上不能求导，它也是主要的优化变量，维数是6维（3维旋转，3维平移）。P是三维空间中的坐标，表示的是第一幅图特征点通过深度图的映射得到的三维坐标值。e(ε)表示目标函数(也叫误差项)，它的维数是3(去掉最后一维的1，其实也是2维)。对位姿进行雅克比求导可知，J是26。
代码解析

初始化部分

// 初始化g2o
// pose 维度为 6, 误差维度为3 (u,v,1)
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  
//线性方程求解器
std::unique_ptr<Block::LinearSolverType>linearSolver(new g2o::LinearSolverCSparse<Block::PoseMatrixType>());  
// 矩阵块求解器
std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) );
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );

Pose顶点的构造

//构建Pose对象
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
        R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
        R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
        R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
//Pose的标志，因为pose的位姿不一定只有一个
pose->setId ( 0 );
//Pose的初值，用g2o里面的李代数，旋转在前，平移在后
pose->setEstimate ( g2o::SE3Quat (R_mat,Eigen::Vector3d ( t.at<double> ( 0,0 ), 
t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )) );
//向优化器加入pose顶点
optimizer.addVertex ( pose );

路标点(Point)的构造

int index = 1;
for ( const Point3f p:points_3d ) 
{
    g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
    point->setId ( index++ );
    point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
    // g2o 中必须设置 marg
    point->setMarginalized ( true ); 
    optimizer.addVertex ( point );
}

相机内参也作为优化变量

//参数: fx,(cx,cy),标识号
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
camera->setId ( 0 );
optimizer.addParameter ( camera );

Point-Pose边的构造

 index = 1;
 for ( const Point2f p:points_2d )
 {
     g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
     edge->setId ( index );
     //在g2o内部实现Point-Pose二元边的构造时,是顶点在前，位姿在后。因此0对应着各个Point，
     //1对应着各个Pose
     edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
     edge->setVertex ( 1, pose );
     //测量值的输入
     edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
     //edge->setParameterId :第二个参数是优化器内添加的参数的id。当你调用addEdge来添加这条边时，
     //会根据第二个参数的id，把相应的参数地址给边，以后边内的成员函数，就根据第一个参数，拿到这个地址。
     edge->setParameterId ( 0,0 );
     edge->setInformation ( Eigen::Matrix2d::Identity() );
     optimizer.addEdge ( edge );
     index++;
 }

提供解析的雅克比导数

void EdgeProjectXYZ2UV::linearizeOplus()
{
    //注意_vertices[1]对应于二元边的Pose
	VertexSE3Expmap* vj=static_cast<VertexSE3Expmap* >(_vertices[1]);
	//estimate()方法为取出估计值
	SE3Quat T(vj->estimate());
	//注意_vertices[0]对应于二元边的Point
	VertexSBAPointXYZ* vi=static_cast<VertexSBAPointXYZ*>(_vertices[0]);
	Vector3d xyz=vi->estimate();
	//map()方法为g2o的点乘方法
	Vector3d xyz_trans=T.map(xyz);
	
	double x=xyz_trans[0];
	double y=xyz_trans[1];
	double z=xyz_trans[2];
	double z_2=z*z;
	
	//parameter(0)对应着camera内参的标识号0
	const CameraParameters* cam=static_cast<const CameraParameters*>(parameter(0));
	Matrix<double,2,3,Eigen::ColMajor>tmp;
	tmp(0,0)=cam->focal_length;
	tmp(0,1)=0;
	tmp(0,2)=-x/z*cam->focal_length;
	
	tmp(1,0)=0
	tmp(1,1)=cam->focal_length;
	tmp(1,2)=-y/z*cam->focal_length;
	
	_jacobianOplusXi=-1/z*tmp*T.rotation().toRotationMatrix();
	//注意g2o的雅可比是旋转在前，平移在后，书上是平移在前，旋转在后
	_jacobianOplusXi(0,0)=x*y/z_2*cam->focal_length;
	....
	_jacobianOplusXi(0,3)=-1./z*cam->focal_length;
	.....
	.....
	_jacobianOplusXi(1,5)
}

优化求解

optimizer.initializeOptimization();
optimizer.optimize ( 100 );
pose->estimate();  //pose位姿的最终结果

2.ICP例子(3D-3D)
目标函数： $e(ε)=P_1-K*exp(ε^{*}*P_2)$ ,其中 $P_1$ 是第一幅图特征点对应的3D点， $P_2$ 是第二幅图特征点对应的3D点， $exp(ε^{*})$ 是相机的位姿，是6维，误差项的维度是3维。解析雅可比导数的维度是3*6(思考一下为什么?)。由于g2o里面并没有Point-Point的边的实现，需要我们自己实现。

边的类重写

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}
    
    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

主体函数说明

    // pose维度为 6, 误差 维度为 3 [x,y,z]
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  
    // 线性方程求解器
    std::unique_ptr<Block::LinearSolverType>linearSolver(new g2o::LinearSolverEigen<Block::PoseMatrixType>());
    // 矩阵块求解器
    std::unique_ptr<Block> solver_ptr (new Block(std::move(linearSolver)));
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::move(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    pose->setId(0);
    pose->setEstimate( g2o::SE3Quat(Eigen::Matrix3d::Identity(),Eigen::Vector3d( 0,0,0 )) );
    optimizer.addVertex( pose );
    
    // edges
    int index = 1;
    vector<EdgeProjectXYZRGBDPoseOnly*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
            Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) );
        edge->setId( index );
        //一元边，0标注位表示相机的Pose
        edge->setVertex( 0, dynamic_cast<g2o::VertexSE3Expmap*> (pose) );
        //观测值
        edge->setMeasurement( Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z) );
        edge->setInformation( Eigen::Matrix3d::Identity()*1e4 );
        optimizer.addEdge(edge);
        index++;
        edges.push_back(edge);
    }

    optimizer.setVerbose( true );
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d( pose->estimate() ).matrix()<<endl;

3.直接法
目标函数： $e(ε)=I_1(p_1)-I_2(p_2) =I_1(\frac{1}{Z_1}*K*P)-I_2(\frac{1}{Z_2}*K*exp(ε^{*})*P)$ ,
误差项的维度为1，优化变量为6，雅可比导数为1*6。由于g2o内部并没有最小化光影误差的边，需要我们自己去实现

边的类重写

class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect() {}

    EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
        : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
	        //经过在灰度图中插值获取得的灰度值getPixelValue（x,y）减去测量值灰度值
	        //这里和公式不太一样，是I2-I1，因此雅可比导数和书上就不太一样，差一个负数
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        //2.1 位姿估计，得到空间坐标系3D坐标
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double invz = 1.0/xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        
        //关于u的梯度
        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    //getPixelValue函数通过双线性差值获得浮点坐标对应插值后的像素值
    inline float getPixelValue ( float x, float y )
    {
        uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
        float xx = x - floor ( x ); 
        float yy = y - floor ( y );
        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]
                 );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

主体函数的说明

    // 求解的向量是1*6的
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; 
    std::unique_ptr<DirectBlock::LinearSolverType> linearSolver(new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>());
    std::unique_ptr<DirectBlock> solver_ptr (new DirectBlock(std::move (linearSolver)));
    // L-M
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr) ); 
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        //参数 第一幅图像的三维值，相机内参，第二、三....的灰度值
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
        );
        edge->setVertex ( 0, pose );
        //测量值，第一幅图的灰度值
        edge->setMeasurement ( m.grayscale );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();

结语：到这里G2O基本使用就介绍完毕了。以上就是我本人这几天对G2O的理解，对里面有的地方解释错误的话，希望大家批评指正。感谢大家
批注：以上的例子是来源于高翔博士的《视觉SLAM十四讲》和一个博主的文章(不过尴尬的是我不知道那个博主的链接在哪，有找到的小伙伴希望告诉我一下。)（文中的SLAM的G2O说明就是那位博主的文章）。转载请附原文链接！！！
博主的地址：g2o库的学习与使用

加粗样式