本文详细介绍了g2o库中边的概念，包括边的头文件、一元边、二元边和多元边的区别。重点讲解了边的参数设置，如测量值维度、数据类型和顶点类型，并给出了定义边的模板示例。通过3D-2D PnP问题的示例，展示了如何重写`computeError()`和`linearizeOplus()`函数以计算误差和雅克比矩阵。此外，还演示了如何将边添加到图中并设置观测值、顶点和信息矩阵。
g2o边的编程
1. g2o的边
1.1 边的头文件
#include<g2o/g2o/core/hyper_graph.h>
#include<g2o/g2o/core/optimizable_graph.h>
#include<g2o/g2o/core/base_edge.h>
BaseUnaryEdge：一元边
BaseBinaryEdge：两元边
BaseMultiEdge : 多元边
1.2 几元边的区别
一元边为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是常见的边；多元边理解为一条边可以连接多个（3个以上）顶点。
1.3 边的参数
以二元边为例：
分析参数：D, E, VertexXi, VertexXj，他们的分别代表：
D 是 int 型，表示测量值的维度 （dimension）
E 表示测量值的数据类型
VertexXi，VertexXj 分别表示不同顶点的类型
1.3.1 示例
用边表示三维点投影到图像平面的重投影误差，可设置输入参数如下
BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
第1个2是说测量值是2维的，也就是图像像素坐标x,y的值，对应测量值的类型是Vector2D，两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ，和李群位姿VertexSE3Expmap
1.4 定义边重写成员函数
1.4.1 重写成员函数
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;// 分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以virtual
void computeError();// 非常重要，是使用当前顶点值计算的测量值与真实测量值之间的误差
virtual void linearizeOplus();// 非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是Jacobian矩阵
1.4.2重要的成员变量及函数
_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆
2. 定义边
2.1 模板示例
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;
// ...
/*
_jocobianOplusXj(pos, pos) = something;
...
*/
}
private:
// data
}
注意：
computeError()，linearizeOplus()
2.2 具体示例
slambook2中ch6中g2oCurveFitting.cpp
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 计算曲线模型误差
void computeError()
{
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 bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x 值， y 值为 _measurement
};
2.3 示例：3D-2D的PnP
代码头文件：
2.3.1 computeError()函数重写
#include<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()));
表示：误差 = 观测 - 投影
a. cam_map()函数
#include<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;
}
b. .map()函数
头文件
#include<g2o/types/sim3/sim3.h>
把世界坐标系下三维点变换到相机坐标系
Vector3 map (const Vector3& xyz) const {
return s*(r*xyz) + t;
}
v1->estimate().map(v2->estimate())
是用V1估计的pose把V2代表的三维点，变换到相机坐标系下。
2.3.2 linearizeOplus()函数重写
用来定义雅克比矩阵
3.向图中添加边
3.1 一元边的添加方法
3.1.1 示例
slambook2/ch6/g2oCurveFitting.cpp
// 往图中增加边
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge( edge );
}
a. setMeasurement()
观测值就是实际观测到的数据点。对于视觉SLAM来说是观测到的特征点坐标
3.2 二元边的添加方法
3.2.1 示例
slambook2/ch7/pose_estimation_3d2d.cpp
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
optimizer.addEdge ( edge );
index++;
}
a. setMeasurement()
p来自向量points_2d，也就是特征点的图像坐标(x,y)
setVertex 有两个一个是 0 和 VertexSBAPointXYZ 类型的顶点，一个是1 和pose
0和1指代顶点不可互换(限于**#include<g2o/types/sba/types_six_dof_expmap.h>**实现的示例EdgeProjectXYZ2UV，示例的定义已经被固定，实际应用中自己继承Edge父类实现)
#include<g2o/core/hyper_graph.h>
中
// set the ith vertex on the hyper-edge to the pointer supplied
void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
_vertices[i] 里的i就是这里的0和1
g2o::EdgeProjectXYZ2UV的定义
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
.....
//李群相机位姿v1
const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
_vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点，也就是三维点，_vertices[1] 对应的是VertexSE3Expmap 类型的顶点，也就是位姿pose。因此前面 1 对应的就应该是 pose，0对应的就是三维点。
扫一扫
1146
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
