精选史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节

江南才尽，年少无知！

发布时间 2023.08.24阅读数 1428 评论数 0

一、前言
通过上一篇博客，已经完成顶点(Vertex)的讲解，下面来看看边(Edge)。其实呢，是比较类似的，但是相对而言g2o的边比顶点稍微复杂一些，不过没有关系，因为编程的都有固定的格式，只需要姑规规矩矩的来就行了。接下来主要分成如下几个部分进行讲解：1、初步认识g2o的边；2、如何自定义g2o的边；3、如何向图中添加边；

在源码 https://github.com/RainerKuemmerle/g2o 的 doc 目录中，可以看到一下 g2o.pdf 文件，可以找到下图：

如果对Jacobian matrix(雅可比矩阵)不太熟悉的朋友，建议先阅读下面这篇博客，下面的讲解涉及较多该知识。
推荐：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解
二、初步认识g2o的边
前面讲解顶点的时候，还专门去追根溯源查找顶点类之间的继承关系，边其实也是类似的，这些代码位于

g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

之中，头文件下就能看到这些继承关系了，我们就不像之前顶点那样一个个去追根溯源了，如果有兴趣你可以自己去试试看。我们主要关注一下上图红框内的三种边。即：BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge → 分别表示一元边，两元边，多元边。那么他们有什么区别呢？

一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个(3个以上)顶点，如下图所示：

下面我们来看看他们的参数有什么区别？主要就是几个参数→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();

(1)read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
( 2 ) c o m p u t e E r r o r ： \color{blue} (2)computeError：(2)computeError：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
( 2 ) l i n e a r i z e O p l u s ： \color{blue} (2)linearizeOplus：(2)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中边的基本类型、重要的成员变量和成员函数，那么如果要定义边，具体是怎么编程呢？下面是一个参考的模板:

 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;
      }      
      private:
      // data
  }

可以很明显的看到，最重要的就是computeError()，linearizeOplus()两个函数了。这里看起来还是比较容易的，下面来看一个比较简单的例子，代码位于：https://github.com/gaoxiang12/slambook2/blob/master/ch6/g2oCurveFitting.cpp
部分代码如下所示

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    virtual void computeError() override {
      const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
    }

    // 计算雅可比矩阵
    virtual void linearizeOplus() override {
      const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
      const Eigen::Vector3d abc = v->estimate();
      double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
      _jacobianOplusXi[0] = -_x * _x * y;
      _jacobianOplusXi[1] = -_x * y;
      _jacobianOplusXi[2] = -y;
    }

    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

这个是个一元边，主要是定义误差函数了，如上所示，还是比较简单的，如果不了解Jacobian matrix(雅可比矩阵)的，可以阅读本人前面给出的链接，里面有详细的讲解。下面是一个复杂一点例子。

四、二元边解析
3D-2D点的PnP 问题，也就是最小化重投影误差问题，这个问题非常常见，使用最常见的二元边，弄懂了这个基本跟边相关的代码也差不多都一通百通了。代码位于g2o/types/sba/types_six_dof_expmap.h之中，如下：

//继承了BaseBinaryEdge类，观测值是2维，类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //1. 默认初始化
    EdgeProjectXYZ2UV();
    //2. 计算误差
    void computeError()  {
      //李群相机位姿v1
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      // 顶点v2
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      //相机参数
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
     //误差计算，测量值减去估计值，也就是重投影误差obs-cam
     //估计值计算方法是T*p,得到相机坐标系下坐标，然后在利用camera2pixel()函数得到像素坐标。
      Vector2D obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }
    //3. 线性增量函数，也就是雅克比矩阵J的计算方法
    virtual void linearizeOplus();
    //4. 相机参数
    CameraParameters * _cam; 
    bool read(std::istream& is);
    bool write(std::ostream& os) const;
};

有一个地方比较难理解：

_error = obs - cam->cam_map(v1->estimate().map(v2->estimate()));

看起来可能没有那么好懂，其实就是：误差 = 观测 − 投影 \color{blue} 误差 = 观测 - 投影误差=观测−投影，下面来捋捋思路。先来看看cam_map 函数，它的定义在 g2o/types/sba/types_six_dof_expmap.cpp 之中，cam_map 函数功能是把相机坐标系下三维点(输入)用内参转换为图像坐标(输出)，具体代码如下所示:

Vector2  CameraParameters::cam_map(const Vector3 & trans_xyz) const {
  Vector2 proj = project2d(trans_xyz);
  Vector2 res;
  res[0] = proj[0]*focal_length + principle_point[0];
  res[1] = proj[1]*focal_length + principle_point[1];
  return res;
}

然后看 .map函数，它的功能是把世界坐标系下三维点变换到相机坐标系，函数在 g2o/types/sim3/sim3.h 具体定义是:

      Vector3 map (const Vector3& xyz) const {
        return s*(r*xyz) + t;
      }

因此下面这个代码

v1->estimate().map(v2->estimate())

就是用V1估计的pose把V2代表的三维点，变换到相机坐标系下。前面主要是对computeError() 的理解，还有一个很重要的函数就是linearizeOplus()，用来定义雅克比矩阵→重投影误差（δu δv）对位姿（R, t）的导数；

摘取相关代码(来自g2o/g2o/types/sba/types_six_dof_expmap.cpp)如下所示：

void EdgeProjectXYZ2UV::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z*z;

  const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));

  Matrix<number_t,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();

  _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
  _jacobianOplusXj(0,2) = y/z *cam->focal_length;
  _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
  _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
  _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

这里的代码看起来还是比较复杂，不过没有关系，再了解了Jacobian matrix(雅可比矩阵) 之后，再进行详细的分析。如果不了解的朋友请阅读下面这篇博客。
推荐：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解这里需要注意一个点，根据博客得到雅克比矩阵(45式)

这是因为 se(3) 的定义方式是旋转在前，平移在后时，只要把这个矩阵的前三列与后三列对调即可。

五、结语
通过该篇博客，对g2o(图优化)中的边(Edge)进行了纤细的讲解，简单的说，只需要实现 computeError()，linearizeOplus()两个函数就可以，下篇博客，会进行一个整理示例的讲解，基于 slam14讲。

SLAM 图优化 G2O Edge 编程细节

转载原出处：https://blog.csdn.net/weixin_43013761/article/details/127085425

打赏 0

上一篇：史上最简SLAM零基础解读(7) - Jacobian matrix(雅可比矩阵) → 理论分析与应用详解(Bundle Adjustment)

下一篇：史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)

精选史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节

江南才尽，年少无知！

为你推荐

精选多传感器特征融合——KM算法原理及C++代码实现（二）

HD-Map lanelet2(3)

旭日X3派学习笔记3：旭日X3派和STM32F103联合项目开发1.0

精选【自动驾驶】路径规划算法Dijkstra与A*

Apollo planning之hybrid A*

自动驾驶技术（0）——自动驾驶的大脑--控制工程篇

关于作者

江南才尽，年少无知！

21

0

0

2

精选目标追踪00-01：FairMOT(实时追踪)-资源下载（前奏准备）

目标追踪00-02：FairMOT(实时追踪)-官方数据训练测试

语义实例分割1-01：snake(实时实例分割))-资源下载（前奏准备）

相关推荐

精选大模型之Bloom&LLAMA----RHLF(强化学习)

ROS实现无人驾驶控制算法（一）——pure pursuit

Apollo planning之PiecewiseJerkPathOptimizer

精选混合A*算法详解（一）路径搜索

电动汽车术语

笛卡尔坐标系转frenet坐标系

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

精选史上最简SLAM零基础解读(10.3) - g2o(图优化)→边(Edge)编程细节

江南才尽，年少无知！

为你推荐

精选多传感器特征融合——KM算法原理及C++代码实现（二）

HD-Map lanelet2(3)

旭日X3派学习笔记3：旭日X3派和STM32F103联合项目开发1.0

精选【自动驾驶】路径规划算法Dijkstra与A*

Apollo planning之hybrid A*

自动驾驶技术（0）——自动驾驶的大脑--控制工程篇

评论（0）

关于作者

江南才尽，年少无知！

21

0

0

2

精选目标追踪00-01：FairMOT(实时追踪)-资源下载（前奏准备）

目标追踪00-02：FairMOT(实时追踪)-官方数据训练测试

语义实例分割1-01：snake(实时实例分割))-资源下载（前奏准备）

相关推荐

精选大模型之Bloom&LLAMA----RHLF(强化学习)

ROS实现无人驾驶控制算法（一）——pure pursuit

Apollo planning之PiecewiseJerkPathOptimizer

精选混合A*算法详解（一）路径搜索

电动汽车术语

笛卡尔坐标系转frenet坐标系

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板