当前位置：首页 > backend >正文

在g2o中，顶点（Vertex）和边（Edge）插入到概率图的流程

backend 2025/5/6 12:28:40

在g2o中，顶点（Vertex）和边（Edge）通过构建图模型描述优化问题，插入到概率图的流程可分为以下核心步骤：

1. 顶点插入流程

(1) 创建顶点对象

// 以SE3位姿顶点为例
g2o::VertexSE3* vertex = new g2o::VertexSE3();

说明：根据问题类型选择顶点类型（如VertexSE3、VertexPointXYZ等）。

(2) 设置顶点初始值

Eigen::Isometry3d initial_pose = Eigen::Isometry3d::Identity();
vertex->setEstimate(initial_pose);  // 设置初始位姿

要点：初始值需合理，否则可能导致优化发散。

(3) 分配唯一ID

vertex->setId(vertex_id);  // ID需全局唯一，通常从0递增

注意：ID用于在图中标识顶点，冲突会导致运行时错误。

(4) 固定或优化选择

vertex->setFixed(true);  // 固定顶点（如首帧位姿）

应用场景：先验信息或无需优化的顶点可固定。

(5) 添加至优化器

optimizer.addVertex(vertex);

2. 边插入流程

(1) 创建边对象

// 以二元边（连接位姿顶点和路标顶点）为例
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();

类型选择：

一元边（BaseUnaryEdge）：单顶点约束（如先验位姿）
二元边（BaseBinaryEdge）：双顶点约束（如重投影误差）
多元边（BaseMultiEdge）：多顶点复杂约束

(2) 连接顶点

edge->setVertex(0, dynamic_cast<g2o::VertexSE3*>(optimizer.vertex(pose_id)));  // 位姿顶点
edge->setVertex(1, dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(point_id)));  // 路标顶点

索引规则：setVertex(k, vertex)中k对应边定义时的顶点顺序。

(3) 设置测量值

Eigen::Vector2d measurement(pixel_x, pixel_y);
edge->setMeasurement(measurement);

作用：观测数据（如像素坐标、传感器读数）作为误差计算基准。

(4) 设置信息矩阵

Eigen::Matrix2d information = Eigen::Matrix2d::Identity() * 1.0 / sigma2;
edge->setInformation(information);

数学意义：信息矩阵为协方差矩阵的逆，反映测量置信度（如像素噪声越大，权重越低）。

(5) 鲁棒核函数（可选）

g2o::RobustKernelHuber* robust_kernel = new g2o::RobustKernelHuber;
edge->setRobustKernel(robust_kernel);

用途：抑制异常值影响（如Huber核降低大误差项的权重）。

(6) 添加至优化器

optimizer.addEdge(edge);

3. 实际应用中的关键设计

(1) 顶点与边类型选择

场景	顶点类型	边类型
2D SLAM位姿优化	`VertexSE2`	`EdgeSE2`（里程计约束）
3D BA优化	`VertexSE3`、`VertexPointXYZ`	`EdgeProjectXYZ2UV`（重投影误差）
传感器融合（GPS+IMU）	`VertexSE3`	`EdgeSE3Prior`（先验约束）

(2) 自定义边实现

若内置边类型不满足需求，需继承基类并实现以下函数：

class CustomEdge : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexSE3, VertexPointXYZ> {
public:// 必须实现：误差计算void computeError() override {// 根据顶点估计值计算误差}// 可选实现：手动雅可比（否则自动数值求导）void linearizeOplus() override {// 计算对顶点的偏导数}
};

(3) 工程优化技巧

批量插入：大规模数据时预分配内存（如optimizer.setMethod()配置稀疏求解器）。
动态增删：使用optimizer.removeVertex()/removeEdge()处理动态环境。
多线程：编译时启用OpenMP加速（需g2o配置支持）。

4. 完整代码示例

// 初始化优化器
g2o::SparseOptimizer optimizer;
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::make_unique<g2o::BlockSolverX>(std::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));
optimizer.setAlgorithm(solver);// 添加顶点（位姿）
g2o::VertexSE3* v_pose = new g2o::VertexSE3();
v_pose->setId(0);
v_pose->setEstimate(Eigen::Isometry3d::Identity());
optimizer.addVertex(v_pose);// 添加顶点（路标点）
g2o::VertexPointXYZ* v_point = new g2o::VertexPointXYZ();
v_point->setId(1);
v_point->setEstimate(Eigen::Vector3d(1.0, 2.0, 3.0));
optimizer.addVertex(v_point);// 添加边（重投影误差）
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setVertex(0, v_pose);
edge->setVertex(1, v_point);
edge->setMeasurement(Eigen::Vector2d(100.0, 200.0));
edge->setInformation(Eigen::Matrix2d::Identity() * 100);
optimizer.addEdge(edge);// 执行优化
optimizer.initializeOptimization();
optimizer.optimize(10);