从零开始一起学习SLAM | 掌握g2o边的代码套路

点“计算机视觉life”关注，置顶更快接收消息！

---

小白：师兄，g2o框架《从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码》，以及顶点《从零开始一起学习SLAM | 掌握g2o顶点编程套路》我都学完啦，今天给我讲讲g2o中的边吧！是不是也有什么套路？

师兄：嗯，g2o的边比顶点稍微复杂一些，不过前面你也了解了许多g2o的东西，有没有发现g2o的编程基本都是固定的格式（套路）呢？

小白：是的，我现在按照师兄说的g2o框架和顶点设计方法，再去看g2o实现不同功能的代码，发现都是一个模子出来的，只不过在某些地方稍微改改就行了啊

师兄：是这样的。我们来看看g2o的边到底是咋回事。

初步认识g2o的边

师兄：在《g2o: A general Framework for (Hyper) Graph Optimization》这篇文档里，我们找到那张经典的类结构图，里面关于边（edge）的部分是这样的，重点是下图中红色框内。

上一次我们讲顶点的时候，还专门去追根溯源查找顶点类之间的继承关系，边其实也是类似的，我们在g2o官方GitHub上这些
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

头文件下就能看到这些继承关系了，我们就不像之前顶点那样一个个去追根溯源了，如果有兴趣你可以自己去试试看。我们主要关注一下上面红框内的三种边。

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

小白：他们有啥区别啊？
师兄：一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点

一个比较丑的示例

下面我们来看看他们的参数有什么区别？你看主要就是 几个参数：D, E, VertexXi, VertexXj，他们的分别代表：

D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型

比如我们用边表示三维点投影到图像平面的重投影误差，就可以设置输入参数如下：

 BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

你说说看 这个定义是什么意思？
小白：首先这个是个二元边。第1个2是说测量值是2维的，也就是图像像素坐标x,y的差值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap？

师兄：对的，就是这样~当然除了输入参数外，定义边我们通常需要复写一些重要的成员函数
小白：听着和顶点类似哦，也是复写成员函数，顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl，边的话是不是也差不多？
师兄：边和顶点的成员函数还是差别比较大的，边主要有以下几个重要的成员函数

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

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

除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：

_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆

后面我们写代码的时候回经常遇到他们的。

如何自定义g2o的边？

小白：前面你介绍了g2o中边的基本类型、重要的成员变量和成员函数，那么如果我们要定义边的话，具体如何编程呢？
师兄：我这里正好有个模板给你看看，基本上定义g2o中的边，就是如下套路：

 class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type>
  {
   
      public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW      
      myEdge(){
   }     
      virtual bool read(istream& in) {
   }
      virtual bool write(ostream& out) const {
   }      
      virtual void computeError() override
      {
   
          // ...
          _error = _measurement - Something;
      }      
      virtual void linearizeOplus() override
      {
   
          _jacobianOplusXi(pos, pos) = something;