g2o优化顶点和边1 2 3 （长文）

小绿最近在学习视觉SLAM十四讲里面的程序，前两天跑来问我关于g2o优化库怎么使用，这两天小白整理网上的一些资料，与小伙伴分一起分享一下。

g2o整体架构

首先来看一张关于g2o整体结构的结构图

这张图最好跟着画一下，这样能更好的理解和掌握，例如我第一次看的时候根本没有注意说箭头的类型等等的细节。

那么从图中我们其实比较容易的就看出来整个库里面较为重要的类之间的继承以及包含关系，也可以看出整个框架里面最重要的东西就是SparseOptimizer这个类（或者说实例）

顺着图往上看，可以看到我们所使用的优化器最终是一个超图（hyperGrahp），而这个超图包含了许多顶点（Vertex）和边（Edge）。这两个类型是我们在看程序和写程序中比较关注的东西了，g2o不像Ceres，内部很多东西其实作者都已经写好了，调用起来非常方便，但是同时，我们也失去了一个比较完整的了解内部关系的机会，不过这个东西我们可以通过看内部的实现补回来，顺便看看大牛写的代码～

在图优化中，顶点代表了要被优化的变量，而边则是连接被优化变量的桥梁，因此，也就造成了说我们在程序中见得较多的就是这两种类型的初始化和赋值。

在整个优化过程中，顶点的值会越来越趋近于最优值，优化完毕后则可以将顶点的优化值作为最优值进行使用；边则是连接顶点的类型，在SLAM问题中，一般是边连接要被优化的空间点（Point）和机器人的位姿（Pose），当然，边还可以连接一个顶点（类似与参数估计，边的数量由量测的数量决定），也可以连接多个顶点，边在图优化中的一个很大的作用就是计算误差，同时计算该误差对于被优化变量的jacobian矩阵，也是比较重要的存在。

自顶点（Vertex）和边（Edge）

在使用g2o的时候最先接触的概念就是顶点和边了，因此接下来我们将介绍这两者，同时看看程序中对两者都留了什么接口。

1. g2o提供的顶点vertex

1）  李代数位姿

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>

继承于BaseVertex这个模板类 
需要设置的模板参数：

• 参数6 ：SE3Quat类型为六维，三维旋转，三维平移

• 参数SE3Quat ：该类型旋转在前，平移在后，注意：类型内部使用的其实是四元数，不是李代数

该顶点需要设置的参数：

g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
vSE3->setId(pKFi->mnId);
vSE3->setFixed(pKFi->mnId==0);

2） 空间点位置

class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>

该顶点需要设置的参数：

g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
vPoint->setId(id);
vPoint->setMarginalized(true);

2. g2o提供的边edge

1）  Point-Pose 二元边（PointXYZPointXYZ-SE3边） 
既要优化MapPoints的位置，又要优化相机的位姿

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

继承于BaseBinaryEdge这个二元边模板类 
需要设置的模板参数：

• 参数2 ：观测值（这里是3D点在像素坐标系下的投影坐标）的维度

• 参数Vector ：观测值类型，piexl.x，piexl.y

• 参数VertexSBAPointXYZ：第一个顶点类型

• 参数VertexSE3Expmap ：第二个顶点类型

该边需要设置的参数：

g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
e->setMeasurement(obs);const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
e->fx = pKFi->fx;
e->fy = pKFi->fy;
e->cx = pKFi->cx;
e->cy = pKFi->cy;

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

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

该继承于BaseUnaryEdge这个一元边模板类，需要设置的模板参数如上

该边需要设置的参数：

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);

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

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

需要设置的参数如下：

Se2 measure_se2 = pMsrOdo->se2;
g2o::Matrix3D covariance = toEigenMatrixXd(pMsrOdo->info).inverse();
Eigen::AngleAxisd rotz(measure_se2.theta, Eigen::Vector3d::UnitZ());
g2o::SE3Quat relativePose_SE3Quat(rotz.toRotationMatrix(), Eigen::Vector3d(measure_se2.x, measure_se2.y, 0));
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();
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);

我们在用g2o的时候，不会一帆风顺的就能适合自身机器人的实际情况，总会遇到自己独特的顶点类型和边类型，此时我们需要对顶点和边进行重写，那么重写也比较简单，这里简单进行记录。

在整体框架图中，可以看到不管是顶点还是边，都可以说是继承自baseXXX这个类的，因此我们在自定义的时候，也可以仿照着继承这两个类，当然也可以继承自g2o中较为“成熟”的类，不管怎样，都要重写下述的函数。

自定义定点

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比较重要，我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要写对，否则会造成一直优化却得不到好的优化结果的现象。

自定义边

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框架要调用的函数进行覆写，这些函数用户可以声明为实函数（即不加virtual），但是笔者还是建议声明为虚函数。

顺着整体结构图往下看，可以看到这部分其实算是整个g2o里面比较隐晦的部分，设计到优化的算法，块求解器，线性求解器等等部分，在程序中，这部分通常位于g2o算法的开头配置部分，一般情况下我们可以随着一个例程进行配置即可，这里对这部分进行了稍微浅显的理解，特意写在这里

求解器

linearSolver线性求解器

我们知道在求解增量方程HdeltaX=-b的时候，通常情况下想到线性求解，很简单嘛，deltaX=-H.inv*b，的确，当H的维度较小的时候，上述问题变得简单，只需要矩阵的求逆就能解决问题，但是当H的维度较大时，问题变得复杂，此时我们就需要一些特殊的方法对矩阵进行求逆，g2o中主要有图中所示的三种方法，PCG，CSparse和Cholmod方法。

注意，这里说再多，线性求解器仅仅只是完成了一个求解的功能，可以说是整个优化中比较靠后的计算部分了。

BlockSolver块求解器

块求解器是包含线性求解器的存在，之所以是包含，是因为块求解器会构建好线性求解器所需要的矩阵块（也就是H和b），之后给线性求解器让它进行运算，边的jacobian也就是在这个时候发挥了自己的光和热。

这里再记录下一个比较容易混淆的问题，也就是在初始化块求解器的时候的参数问题。

大部分的例程在初始化块求解器的时候都会使用如下的程序代码：

std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

其中的BlockSolver_6_3有两个参数，分别是6和3，在定义的时候可以看到这是一个模板的重命名（模板类的重命名只能用using）

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

其中p代表pose的维度，l表示landmark的维度，且这里都表示的是增量的维度（这里笔者也不是很确定，但是从后续的程序中可以看出是增量的维度而并非是状态变量的维度）。

关于g2o还有很多东西需要学习，小白在后续的学习过程中如果有了新的感悟会继续与大家分享，也希望有更深感悟的小伙伴能够与小白和其他小伙伴们一起分享。

本文部分内容参考

https://blog.csdn.net/wubaobao1993/article/details/79319215

https://blog.csdn.net/hzwwpgmwy/article/details/79884070