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

在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, dy