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

小白:兄弟，我已经学完了g2o框架和顶点。说说今天g2o的优势？有什么套路吗？

兄弟:嗯，g2o的边比顶点稍微复杂一点，但是你早知道很多关于g2o的事情。你有没有发现g2o的编程基本都是固定格式(套路)？

小白:是的，根据师兄说的g2o框架和顶点设计方法，我看了一下g2o实现不同功能的代码，发现都是同一个模具做的，只是有些地方略有变化。

哥哥:是这样的。我们来看看g2o的边缘。

对g2o边缘的初步认识

兄弟:在文档《G2O:一个(Hyper)图优化的通用框架》中，我们找到了经典的类结构图，其中关于边的部分是这样的，重点是下图中的红框。

上一次讲顶点的时候，我们也是回到源码中去找顶点类之间的继承关系。边缘其实差不多。我们在g2o官方GitHub上有这些

g2o/g2o/core/hyper_graph.h

g2o/g2o/core/optimizable _ graph . h

g2o/g2o/core/base_edge.h

你可以在头文件下面看到这些继承关系，所以我们不像前面的顶点那样一个一个的追溯来源。有兴趣可以自己试试。我们主要关注上面红框中的三面。

基本一元边、基本二元边、基本多元边分别代表一元边、二元边和多元边。

小白:它们有什么区别？

兄弟:一维边可以理解为一条边只连接一个顶点，二维边理解为一条边连接两个顶点，这是我们共同的边。多维边可以理解为连接多个(多于三个)顶点的一条边

丑陋的例子

让我们看看它们的参数之间的区别。看，主要有几个参数:D，E，VertexXi，VertexXj，以及它们各自的代表:

d是int类型，表示测量值的维数

e表示测量值的数据类型

顶点21和顶点22分别代表不同顶点的类型

例如，如果我们使用边来表示三维点投影到像平面的重投影误差，我们可以如下设置输入参数:

BaseBinaryEdge<。2，Vector2D，VertexSBAPointXYZ，VertexSE3Expmap >等；

这个定义是什么意思？

小白:首先，这是一个二元边。前2表示测量值是二维的，也就是图像像素坐标x和y的差值，对应的测量值类型是Vector2D，两个顶点，也就是优化变量分别是三维的点VertexSBAPointXYZ和李群的姿态VertexSE3Expmap？

兄弟:没错，就是这样~当然，除了输入参数，我们通常需要复制一些重要的成员函数来定义边

小白:听起来像顶点，也是复制成员函数。顶点更新函数oplusImpl和顶点重置函数setToOriginImpl主要在顶点中复制。就边缘而言是否相似？

兄弟:边和顶点的成员函数差别很大，边主要有以下重要的成员函数

virtualboolread(STD::is tream & amp；is)；

virtualboolwrite(STD::ostream & amp；OS)const；

virtualvoitcomputeerror()；

virtualvoitlinelypexus()；

以下是简要说明

读、写:它们是读和保存功能。一般不需要读/写的，直接声明即可

ComputeError函数:非常重要，它是用当前顶点的值计算出的测量值与实际测量值之间的误差

线性化人函数:非常重要，它是当前顶点值处的误差对优化变量的偏导数，我们称之为雅可比

除了上述成员函数，还有几个重要的成员变量和函数需要解释:

_测量:存储观察值

_错误:存储由computeError()函数计算的错误

_顶点[]:存储顶点信息，如二进制边，_顶点[]的大小为2，存储顺序与调用setVertex( int，vertex)有关，set (0或1)

SetId( int):定义边的数目(决定h矩阵中的位置)

设置测量(类型)函数来定义观察值

设置顶点(int，Vertex)来定义顶点

SetInformation()定义协方差矩阵的逆矩阵

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

g2o的边缘如何定制？

小白:之前你介绍了g2o中边的基本类型、重要成员变量和成员函数。如果要定义边，如何编程？

兄弟:我这里正好有个模板给你看，基本上定义了g2o中的edge，也就是下面的套路:

class myedge:public g2o::BaseBinaryEdge & lt；错误尺寸、错误类型、顶点类型、顶点类型。

{

公共:

本征_制造_对齐_运算符_新

myEdge(){}

virtualboolread(is tream & amp；in){}

virtualboolwrite(ostream & amp；out)const{}

virtualvoidcomputeError()重写

{

// ...

_误差= _测量-某物；

}

virtualvoidlinearizeOplus()覆盖

{

_jacobianOplusXi(pos，pos) =某物；

// ...

/*

_ Jocobianpolusxj(pos，pos) =某物；

...

*/

}

私人:

//数据

}

我们可以发现最重要的函数是computeError()和linearizeOplus()

小白:看起来不难。

兄弟:先看一个简单的例子。地址是

https://github . com/Gao Xiang 12/slambook/blob/master/ch6/g2o _ curve _ fitting/main . CPP

这是一元边，主要定义误差函数。如下图，你可以发现这个例子基本上是上面例子的一个丢失的扩展。感觉这么轻松吗？

//错误模型模板参数:观察维度、类型、连接顶点类型

class curveFittingedge:public g2o::BaseUnareedge & lt；1，双，曲线顶点> 1；

{

公共:

本征_制造_对齐_运算符_新

curvefitingedge(doublex):base UNARYedge()，_x(x){}

//计算曲线模型误差

无效计算错误()

{

constcurfefitingvertex * v = static _ cast & lt；constCurveFittingVertex * >;(_顶点[0])；

const eign::Vector 3d ABC = v->；estimate()；

_error( 0，0) = _measurement - std:: exp( abc( 0，0)*_x*_x + abc( 1，0)*_x + abc( 2，0))；

}

virtualboolread(is tream & amp；in ){}

virtualboolwrite(ostream & amp；out )const{}

公共:

double _ x；// x值，y值是_measurement

};

小白:嗯，这可以理解

兄弟:下面是一个比较复杂的例子。三维2D点的概率神经网络问题，即最小化重投影误差的问题，是非常常见的。用最常见的二进制边，我理解这个基本的边相关代码几乎都是通过的。

代码可以在g2o的GitHub上的这个地方看到

g2o/types/SBA/types _ six _ DOF _ ex pmap . h

这里根据我自己的理解对代码进行注释，方便理解

//继承BaseBinaryEdge类，观察值为2D，类型为Vector2D，顶点分别为3D点和李群姿态

class g2o _ TYPES _ SBA _ apiegeproject XYZ 2 Uv:public basebinaryedge & lt；2，Vector2D，VertexSBAPointXYZ，VertexSE3Expmap >等；{

公共:

EIGEN _ MAKE _ ALIGNED _ OPERATOR _ NEW；

//1.默认初始化

edgeprojectxyz 2 Uv()；

//2.计算误差

voidcomputeError(){

//李群相机姿势v1

constituexse3ex pmap * v1 = static _ cast & lt；constVertexSE3Expmap * & gt(_顶点[1])；

//顶点v2

constituexsbapointxyz * v2 = static _ cast & lt；constVertexSBAPointXYZ * >;(_顶点[0])；

//相机参数

const camera参数* cam

= static _ cast & lt常量参数* >;(参数(0))；

//误差计算，测量值减去估计值，即反投影误差

//通过T*p计算估计值，得到相机坐标系下的坐标，然后利用camera2pixel()函数得到像素坐标。

vector 2d OBS(_ measurement)；

_ error = OBS-cam->；cam_map(v1->;估计()。map(v2->;estimate()))；

}

//3.线性增量函数，即雅可比矩阵j的计算方法。

virtualvoitlinelypexus()；

//4.摄像机参数

CameraParameters * _ cam

boolread(STD::is tream & amp；is)；

bool write(STD::ostream & amp；OS)const；

};

有一个地方很难理解

_ error = OBS-cam->；cam_map(v1->;估计()。map(v2->;estimate()))；

小白:我真的不明白这句话。。

兄弟:其实就是:误差=观察-投射

我给你出点主意。让我们首先看看cam_map函数，它在

g2o/types/SBA/types _ six _ DOF _ ex pmap . CPP

cam_map函数的作用是将相机坐标系中的3d点(输入)转换成带有内部参数的图像坐标(输出)。具体代码如下

vector 2 Camera parameters::cam _ map(const vector 3 & amp；trans_xyz)常量{

vector 2 proj = project 2d(trans _ XYZ)；

Vector2 res

RES[0]= proj[0]*焦距+主点[0]；

[1]=[1]*焦距+原则点[1]；

returnres

}

然后看看。地图功能，它的功能是将世界坐标系中的三维点变换到相机坐标系中，而这个功能是在

g2o/types/sim3/sim3.h

具体定义是

Vector3地图(constVector3 & ampxyz)常量{

返回*(r * XYZ)+t；

}

所以下面的代码

v1->;估计()。map(v2->;估计())

也就是说，由V1估计的姿态被用于将由V2表示的3d点转换到相机坐标系中。

小白:我明白了。我之前忽略了这些东西。没想到他们这么有血缘关系。

兄弟:好吧，我们继续。第一个是对computeError()的理解，另一个重要的函数是linearizeOplus()，用来定义雅可比矩阵

我提取了相关代码(来自:g2o/g2o/types/SBA/types _ six _ DOF _ ex pmap . CPP)并做了标记，相信会更容易理解。

第14讲第169页的雅可比矩阵完全按照书中公式(7.45)和(7.47)编程，不难理解

小白:直接抄书就行了，哈哈

如何给图表添加边？

兄弟:我们说了怎么给图加顶点，可以说很简单。向图中添加边会有更多的内容。先说最简单的例子:加一元边的方法

以下代码来自GitHub，之前还是曲线拟合的例子

slambook/ch6/g2o _ curve _ fitting/main . CPP

//向图形添加边

for(inti = 0；i<。n；i++)

{

curvefitingedge * edge = new curvefitingedge(x _ data[I])；

edge->setId(I)；

edge->setVertex( 0，v)；//设置连接的顶点

edge->setMeasurement(y _ data[I])；//观察值

edge->集合信息(特征::矩阵& ltdouble，1，1 >*同一性()* 1/(w _ sigma * w _ sigma))；//信息矩阵:协方差矩阵的逆矩阵

optimizer . AddEdge(edge)；

}

小白:setMeasurement函数输入的观察值到底是什么意思？

兄弟:这个曲线拟合，观测值其实就是观测数据点。对于视觉SLAM，通常是我们观察到的特征点的坐标。下面举个例子。这个例子比刚才稍微复杂一点，因为是二元边，需要用边连接两个顶点

代码来自GitHub

slambook/ch7/pose _ estimation _ 3d 2d . CPP

index = 1；

for( constPoint2f p:points_2d)

{

g2o::EdgeProjectxyz 2 Uv * edge = new g2o::EdgeProjectxyz 2 Uv()；

edge->setId(索引)；

edge->setVertex ( 0，dynamic _ cast & ltg2o::VertexSBAPointXYZ* >;(optimizer . vertex(index)))；

edge->setVertex ( 1，pose)；

edge->set Measurement(Eigen::vector 2d(p . x，p . y))；

edge->setParameterId ( 0，0)；

edge->setInformation(Eigen::matrix 2d::Identity())；

optimizer . AddEdge(edge)；

index++；

}

小白:这里setMeasurement函数中的P来自向量点_2d，也就是特征点的图像坐标(x，y)。

兄弟:对，这正好呼应了我刚才说的。另外可以看到，setVertex有两个顶点，类型为0和VertexSBAPointXYZ，一个是1和pose。你认为这里的0和1是什么意思？可以互换吗？

小白:0和1应该分别指哪个顶点。直觉告诉我，它们不能互换。我可能需要检查顶点定义部分的代码

兄弟:你的直觉没错！我帮你查过了。你看这是g2o官网中setVertex的定义:

//将超边上的第I个顶点设置为提供的指针

voidsetVertex(size_ti，Vertex * v){ assert(I & lt；_顶点.大小()& amp& amp“索引越界”)；_顶点[I]= v；}

这个代码在

g2o/core/hyper_graph.h

可以在。看，I in _折点[i]在这里是我们的0和1，让我们看看这里边的类型:g2o::EdgeProjectXYZ2UV

的定义，我们之前也发布过，就这两句话

class g2o _ TYPES _ SBA _ apiegeproject XYZ 2 Uv

.....

//李群相机姿势v1

constituexse3ex pmap * v1 = static _ cast & lt；const VertexSE3Expmap * >(_顶点[1])；

//顶点v2

constituexsbapointxyz * v2 = static _ cast & lt；constVertexSBAPointXYZ * >;(_顶点[0])；

可以看到_折点[0]对应的是VertexSBAPointXYZ类型的折点，也就是3D点，而_折点[1]对应的是VertexSE3Expmap类型的折点，也就是姿态。所以1要对应姿态，0要对应三维点。

小白:我明白了。之前没注意这些东西。g2o好像也不会帮我区分顶点类型。以后这里的编程要对应好，不然我找不到出错的原因！谢谢兄弟。今天又是一整天！

编程练习

题目:直接捆绑调整的相机姿态估计。给定三张图片和两个txt文件。txt以timestamp、tx、ty、tz、qx、qy、qz、qw这三个分别对应时间戳、平移、旋转(四元数)的格式存储这三个图片对应的相机初始姿态(Tcw)，而points.txt存储3D点集和点周围的4x4。

x，y，z，灰度1，灰度2，...，灰度16

我们将每个三维点投影到相应的图像中，并利用投影点周围的灰度值与原始窗口的灰度值之差作为误差进行优化。

请使用g2o优化并绘制结果(绘制功能已写好)。

您需要在代码框架中填写顶点和边的定义。如果正确，输出结果如下图所示:

参考:

高翔视觉SLAM十四讲

https://blog . csdn . net/再试一次_稍后/article/details/81813639

代码框架、数据、窗口值的具体顺序、优化目标函数、预期输出结果都已经为您准备好了。微信官方账号“计算机视觉生活”后台回复是:可以得到。

欢迎留言讨论，了解更多视频、文档、参考答案等。关注计算机视觉生活微信官方账号，点击菜单栏中的“知识星球”查看“从零开始学SLAM”星球介绍，快来和其他朋友学习交流吧~

从零开始学SLAM |为什么要学SLAM？

从零开始学SLAM |学SLAM需要学什么？

从零开始学SLAM | SLAM有什么用？

要不要从头学SLAM | C++的新特性？

从头学SLAM为什么要用齐次坐标？

从头开始学习刚体在SLAM | 3D 空之间的旋转

从头学SLAM为什么需要李群和李代数？

从头开始学习SLAM |相机成像模型

学SLAM |不从零开始推公式，如何真正理解极坐标约束？

从头开始学习SLAM |魔术单应矩阵

从头开始学SLAM你好，点云

从头开始学习SLAM向点云添加滤镜

从头开始学习SLAM |点云的平滑正态估计

从零开始学习从SLAM |点云到网格的演变

从零开始学习SLAM了解图形优化，带你一步一步通过g2o代码

从头开始学习SLAM掌握g2o顶点编程例程

零基础小白，计算机视觉如何入门？

结合牛人、牛实验室、牛在SLAM领域的研究成果

我用MATLAB得到了一个2D激光雷达SLAM

形象化理解四元数，希望你永远不要再掉头发

语义SLAM近几年的代表作是什么？

可视化SLAM技术综述

VIO和激光SLAM相关论文分类摘要

学习SLAM对编程的要求有多高？

2018 SLAM，3 D视觉方向求职体验分享

深度学习遇到SLAM |如何基于深度学习评价DeepVO、VINet、VidLoc？