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

在g2o图优化框架中，顶点（Vertex）和边（Edge）的定义与功能的区别

ds 2025/5/4 0:16:13

在g2o图优化框架中，顶点（Vertex）和边（Edge）是构建优化问题的核心组件，两者的定义与功能存在以下关键区别：

1. 作用与本质差异

顶点（Vertex）
代表待优化的变量，例如：
- 位姿（如VertexSE3Expmap表示3D位姿，包含平移和旋转）
- 空间点坐标（如VertexPointXYZ表示3D点）
- 参数（如VertexSBAPointXYZ用于SBA优化）
  在SLAM中，顶点通常是机器人位姿或地图点。
边（Edge）
表示误差项，通过连接顶点定义约束关系，例如：
- 一元边（BaseUnaryEdge）：连接单个顶点（如先验约束）
- 二元边（BaseBinaryEdge）：连接两个顶点（如里程计约束）
- 多元边（BaseMultiEdge）：连接多个顶点（复杂约束）
  在BA优化中，边常表示重投影误差或传感器测量误差。

2. 定义时的代码结构差异

顶点定义

// 模板参数：<优化变量维度, 数据类型>
class CustomVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:// 重置顶点初始值void setToOriginImpl() override { _estimate << 0, 0, 0; }// 顶点更新函数（增量Δx叠加）void oplusImpl(const double* update) override {_estimate += Eigen::Vector3d(update[0], update[1], update[2]);}// 读写函数（若无需磁盘操作可空实现）bool read(std::istream& in) override { return true; }bool write(std::ostream& out) const override { return true; }
};

关键点：

必须重写setToOriginImpl()和oplusImpl()，定义初始化和增量更新逻辑。
模板参数中维度对应优化变量自由度（如3维位姿为6）。

边定义

// 模板参数：<误差维度, 测量值类型, 顶点类型...>
class CustomEdge : public g2o::BaseBinaryEdge<2, Vector2d, VertexSE3, VertexPoint> {
public:// 误差计算（核心）void computeError() override {const VertexSE3* pose = static_cast<VertexSE3*>(_vertices[0]);const VertexPoint* point = static_cast<VertexPoint*>(_vertices[1]);_error = measurement_ - project(pose->estimate(), point->estimate());}// 雅可比矩阵计算（可手动或自动求导）void linearizeOplus() override {// 手动推导对顶点参数的偏导数...}
};

关键点：

必须重写computeError()，定义误差计算逻辑。
可选重写linearizeOplus()，手动指定雅可比矩阵（否则使用数值求导）。
模板参数需指定误差维度（如2D像素误差为2）及连接的顶点类型。

3. 在优化中的角色差异

特性	顶点（Vertex）	边（Edge）
数据存储	存储待优化变量（如位姿、点坐标）	存储测量值（如像素坐标、里程计数据）
信息矩阵	无直接关联	通过`setInformation()`设置协方差逆矩阵
连接关系	独立存在	必须连接至少一个顶点，定义约束关系
优化更新	通过`oplusImpl()`迭代更新	通过误差函数驱动顶点更新

4. 实际应用中的关联

顶点与边的协作
边通过连接顶点构建约束图。例如在BA中：
- 顶点：相机位姿（VertexSE3Expmap）和3D点（VertexSBAPointXYZ）。
- 边：重投影误差边（EdgeProjectXYZ2UV），连接一个位姿顶点和一个点顶点，计算像素坐标误差。
性能影响
- 顶点维度决定Hessian矩阵规模（维度高则计算量大）。
- 边的误差函数和雅可比实现方式（手动/自动求导）影响优化速度。

总结

顶点是待优化的变量载体，边是描述变量间约束关系的误差项。顶点定义关注参数初始化和更新，边定义关注误差计算和导数推导。两者协同构建图模型，通过优化算法迭代求解最优变量值。实际应用中需根据问题特性选择合适的顶点和边类型（如SE3位姿顶点或二元边连接方式）。

如何在g2o中自定义边

1. 确定边类型与继承基类

核心操作：

选择边类型：根据约束关系选择一元边（BaseUnaryEdge）、二元边（BaseBinaryEdge）或多元边（BaseMultiEdge）。

模板参数定义：需明确误差维度、测量值类型及连接的顶点类型。

// 示例：二元边（连接位姿顶点和路标顶点，误差维度3）
class CustomEdge : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexSE3, VertexPointXYZ> {// 类实现...
};

注意事项：

Eigen内存对齐：添加宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW避免内存对齐问题。

2. 实现误差函数（computeError）

作用：根据顶点当前估计值计算残差。
代码示例（重投影误差）：

void computeError() override {const VertexSE3* pose = static_cast<VertexSE3*>(_vertices[0]);  // 位姿顶点const VertexPointXYZ* point = static_cast<VertexPointXYZ*>(_vertices[1]);  // 路标顶点Eigen::Vector3d projected = pose->estimate() * point->estimate();  // 投影到相机坐标系_error = _measurement - projected.head<2>();  // 计算像素误差
}

关键点：

_measurement存储观测值（如像素坐标、IMU数据）。
_error需与模板参数中的误差维度一致。

3. 实现雅可比矩阵（手动或自动）

选项一：手动推导（推荐）

void linearizeOplus() override {const VertexSE3* pose = static_cast<VertexSE3*>(_vertices[0]);const VertexPointXYZ* point = static_cast<VertexPointXYZ*>(_vertices[1]);// 计算对位姿和路标的偏导数（需根据李代数扰动模型推导）Eigen::Matrix<double, 2, 6> J_pose;  // 位姿雅可比（2x6）Eigen::Matrix<double, 2, 3> J_point;  // 路标雅可比（2x3）_jacobianOplus[0] = J_pose;  // 索引对应顶点连接顺序_jacobianOplus[1] = J_point;
}

适用场景：

性能敏感场景（如SLAM实时优化）。
李群（SE3）或复杂误差模型（如Sim3）。

选项二：自动数值求导

省略linearizeOplus函数，g2o自动计算数值导数。
缺点：效率低，仅适用于简单误差模型或调试阶段。

4. 设置边属性与连接关系

关键操作：

绑定顶点：通过setVertex(k, vertex)按顺序连接顶点。

edge->setVertex(0, pose_vertex);  // 位姿顶点为第0个
edge->setVertex(1, point_vertex); // 路标顶点为第1个

设置信息矩阵：反映测量噪声的逆协方差。

Eigen::Matrix2d info = Eigen::Matrix2d::Identity() / sigma2;
edge->setInformation(info);

鲁棒核函数（可选）：抑制异常值影响。

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

5. 调试与验证

验证方法：

数值导数校验：启用g2o的setVerify功能，对比手动雅可比与数值导数是否一致。
```
optimizer.setVerbose(true);
optimizer.setComputeBatchStatistics(true);
```
残差监控：观察优化前后残差变化，确认误差收敛。
可视化工具：使用g2o的save函数导出图结构，用第三方工具（如g2o_viewer）检查连接关系。

扩展：实际工程中的优化技巧

模块化封装：参考hdl_graph_slam，将边插入逻辑封装为独立函数（如addEdgeSE3），提升代码可维护性。
批量处理：大规模问题中使用稀疏矩阵求解器（如LinearSolverCholmod）。
多线程支持：编译时启用OpenMP加速优化迭代。

总结流程图

[选择边类型] → [继承基类] → [实现computeError]  ↓  
[推导雅可比（手动/自动）] → [设置顶点连接]  ↓  
[设置信息矩阵/鲁棒核] → [添加到优化器]  ↓  
[调试验证] → [部署优化]

如何在g2o等图优化框架中，自定义边的误差函数

1. 误差函数的本质与作用

误差函数（Error Function）是连接顶点参数与观测值的数学表达式，其核心目标：

量化预测与实际的偏差：计算顶点参数估计值（如位姿、3D点）与观测数据之间的差异。
驱动参数优化：通过最小化误差函数（常为最小二乘形式），调整顶点参数使预测更贴合实际观测。

2. 误差函数定义步骤

(1) 确定误差维度与类型

维度选择：根据观测数据类型决定误差向量维度。
- 示例：像素坐标误差为2维（x,y），IMU预积分误差为15维。
误差方向：通常定义为误差 = 观测值 - 预测值，与最小二乘形式匹配。

(2) 获取顶点参数

通过_vertices数组访问连接的顶点参数，需进行类型转换：

// 以SE3位姿顶点和3D点顶点为例
const VertexSE3* pose_vertex = static_cast<VertexSE3*>(_vertices[0]);
const VertexPointXYZ* point_vertex = static_cast<VertexPointXYZ*>(_vertices[1]);
Eigen::Vector3d point = point_vertex->estimate();  // 获取3D点坐标

(3) 实现误差计算逻辑

根据顶点参数和观测值计算误差向量，需在computeError()中实现：

void computeError() override {// 将3D点投影到相机坐标系Eigen::Vector3d projected = pose_vertex->estimate().map(point);// 计算像素误差（观测值 - 投影坐标）_error = _measurement - camera_model.project(projected);
}

关键参数：

_measurement：观测值（如相机像素坐标、激光雷达点云），需在插入边时通过setMeasurement()设置。
_error：误差向量，维度需与模板参数一致。

(4) 设置信息矩阵

通过setInformation()定义协方差逆矩阵，加权不同观测的置信度：

Eigen::Matrix2d info_matrix = Eigen::Matrix2d::Identity() / sigma_pixel;
edge->setInformation(info_matrix);

应用场景：若某传感器噪声较大（如深度相机边缘噪声），可降低对应信息矩阵权重。

3. 误差函数设计实例

案例：视觉SLAM重投影误差

class EdgeReprojection : public g2o::BaseBinaryEdge<2, Vector2d, VertexSE3, VertexPointXYZ> {
public:void computeError() override {// 获取顶点参数const VertexSE3* v_pose = static_cast<VertexSE3*>(_vertices[0]);const VertexPointXYZ* v_point = static_cast<VertexPointXYZ*>(_vertices[1]);// 坐标变换与投影Vector3d pt_cam = v_pose->estimate() * v_point->estimate();  // 世界系→相机系Vector2d pt_pixel = camera2pixel(pt_cam);                    // 相机系→像素// 误差计算_error = _measurement - pt_pixel;  // 观测像素坐标 - 预测像素坐标}// 相机投影模型（假设为针孔模型）Vector2d camera2pixel(const Vector3d& pt_cam) const {return Vector2d(fx * pt_cam.x() / pt_cam.z() + cx,fy * pt_cam.y() / pt_cam.z() + cy);}
};

案例：IMU预积分误差

class EdgeIMUPreintegration : public g2o::BaseMultiEdge<15, IMUPreintegration> {
public:void computeError() override {// 获取顶点参数（上一时刻位姿、速度、零偏，当前时刻对应参数）const VertexNavState* v_prev = static_cast<VertexNavState*>(_vertices[0]);const VertexNavState* v_curr = static_cast<VertexNavState*>(_vertices[1]);// 计算预积分残差（位置、速度、旋转、零偏等15维）_error = preintegrate(v_prev->estimate(), v_curr->estimate()) - _measurement;}
};

4. 高级技巧与注意事项

(1) 鲁棒核函数

通过添加Huber、Cauchy等核函数抑制异常值影响：

g2o::RobustKernelHuber* robust_kernel = new g2o::RobustKernelHuber;
robust_kernel->setDelta(1.0);  // 设定阈值
edge->setRobustKernel(robust_kernel);

(2) 雅可比矩阵处理

手动计算：在linearizeOplus()中实现，适用于SE3等李群参数（需扰动模型）。

void linearizeOplus() override {// 计算对位姿的雅可比（使用李代数左乘扰动）Eigen::Matrix<double, 2, 6> J_pose;// 计算对3D点的雅可比Eigen::Matrix<double, 2, 3> J_point;_jacobianOplus[0] = J_pose;_jacobianOplus[1] = J_point;
}

自动求导：省略linearizeOplus()，g2o自动计算数值导数（效率较低，适合调试）。

(3) 动态误差函数

若误差模型随参数变化（如自适应权重），可在每次迭代时动态调整：

void computeError() {// 根据当前顶点参数计算动态权重double weight = compute_adaptive_weight(pose_vertex->estimate());_error = weight * (_measurement - prediction);
}

5. 调试与验证方法

数值导数校验：启用g2o调试模式，对比手动雅可比与数值导数是否一致。
```
optimizer.setVerbose(true);
optimizer.setComputeBatchStatistics(true);
```
残差可视化：输出优化前后误差值，观察收敛趋势。
仿真测试：在合成数据中验证误差函数逻辑正确性。

总结

定义误差函数的核心流程为：确定误差维度 → 获取顶点参数 → 计算预测值与观测值的差值 → 设置信息矩阵与鲁棒核。实际应用中需结合具体传感器模型（如相机畸变、IMU噪声）调整误差计算逻辑，并通过雅可比矩阵优化加速收敛。对于复杂系统（如多传感器融合），建议参考VINS-Mono、ORB-SLAM3等开源框架的误差函数设计实现。

查看全文

http://www.xdnf.cn/news/2912.html