2. g2o中的顶点与边类型详解

任务目标

在g2o中，顶点（Vertex）和边（Edge）是图优化模型的两个核心抽象。本案例将梳理g2o内置的常用顶点与边类型，并演示如何自定义一个简单的顶点和一元边，完成最小可运行的优化闭环。

g2o常用内置类型速查

自定义顶点与一元边

当内置类型无法表达自定义状态量（如相机内参、IMU零偏）时，需要继承BaseVertex和BaseUnaryEdge。下面以估计一维标量参数 x 为例，给出最小完整实现：

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <Eigen/Core>
#include <iostream>
#include <cmath>

// 1. 自定义顶点：一维标量
class VertexScalar : public g2o::BaseVertex<1, double> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() override { _estimate = 0.0; }
    virtual void oplusImpl(const double* update) override { _estimate += update[0]; }
    virtual bool read(std::istream& is) override { is >> _estimate; return true; }
    virtual bool write(std::ostream& os) const override { os << _estimate; return true; }
};

// 2. 自定义一元边：观测值 = x^2
class EdgeSquared : public g2o::BaseUnaryEdge<1, double, VertexScalar> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void computeError() override {
        const VertexScalar* v = static_cast<const VertexScalar*>(_vertices[0]);
        _error[0] = v->estimate() * v->estimate() - _measurement;
    }
    virtual void linearizeOplus() override {
        const VertexScalar* v = static_cast<const VertexScalar*>(_vertices[0]);
        _jacobianOplusXi[0] = 2.0 * v->estimate();
    }
    virtual bool read(std::istream& is) override { is >> _measurement; return true; }
    virtual bool write(std::ostream& os) const override { os << _measurement; return true; }
};

int main() {
    // 3. 构建优化器
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<1, 1>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(false);

    // 4. 添加顶点，初值 x=3
    auto* v = new VertexScalar();
    v->setId(0);
    v->setEstimate(3.0);
    optimizer.addVertex(v);

    // 5. 添加边：假设观测值为 4（即真实 x=2 或 x=-2）
    auto* e = new EdgeSquared();
    e->setId(0);
    e->setVertex(0, v);
    e->setMeasurement(4.0);
    e->setInformation(Eigen::Matrix<double,1,1>::Identity());
    optimizer.addEdge(e);

    // 6. 执行优化
    optimizer.initializeOptimization();
    optimizer.optimize(50);

    std::cout << "优化后 x = " << v->estimate() << std::endl;
    std::cout << "残差 = " << e->chi2() << std::endl;
    return 0;
}

编译与运行

# CMakeLists.txt 关键内容
# find_package(g2o REQUIRED)
# include_directories(${G2O_INCLUDE_DIR})
# target_link_libraries(your_target g2o_core g2o_stuff g2o_types_slam3d)

编译后运行程序，观察输出。由于 x^2=4 有两个解 x=±2，初值 x=3 经高斯-牛顿迭代会收敛到最近的局部极小值 x=2。

验证方法与判定标准

• 预期输出：程序打印的 x 值应接近 2.0（误差 < 1e-3），chi2 接近 0（< 1e-6）。这是由代码逻辑直接推出的确定性结果。
• 自定义正确性检查：若将初值改为 -3.0，优化结果应接近 -2.0，验证 linearizeOplus 中雅可比符号正确。
• 类型接口验证：顶点必须实现 setToOriginImpl 和 oplusImpl；边必须实现 computeError。若未实现 linearizeOplus，g2o 会启用数值差分，结果仍收敛但速度较慢，可用于交叉验证解析雅可比的正确性。

常见问题

• 编译错误：确保链接了 g2o_core 和 g2o_solver_eigen 库，且 Eigen 头文件路径正确。
• 不收敛：检查信息矩阵是否为单位阵，初值是否在收敛域内（本例中初值不能为0，否则雅可比为0导致无法迭代）。
• 维度不匹配：BaseVertex<1, double> 的第一个模板参数是估计量的最小表示维度（更新向量维度），第二个是估计量的数据类型，二者必须与 oplusImpl 中使用的维度一致。