1. 问题描述

我们有一系列二维数据点 (xi,yi)(x_i, y_i)，我们相信这些数据点是由一个指数模型生成的，具体形式为：

y=exp⁡(ax2+bx+c)y = \exp(ax^2 + bx + c)

然而，我们的观测值 yiy_i 包含噪声。我们的目标是：根据这些带有噪声的观测数据 (xi,yi)(x_i, y_i)，估计出最可能生成这些数据的模型参数 (a,b,c)(a, b, c) 的值。

这本质上是一个优化问题：寻找一组参数 (a,b,c)(a, b, c)，使得模型预测值 exp⁡(axi2+bxi+c)\exp(ax_i^2 + bx_i + c) 与实际观测值 yiy_i 之间的总误差最小（通常是最小化误差的平方和）。

2. 思考：如何用图优化解决？

我们要将这个问题"翻译"成 g2o 的语言——图。一个图由 顶点 (Vertices) 和 边 (Edges) 组成。

  • 顶点 (Vertex) 代表什么？

    • 顶点代表了我们要优化的 未知变量。
    • 在这个问题中，未知变量就是曲线的参数 (a,b,c)(a, b, c)。这三个参数需要一起被估计，它们共同构成了一个状态。
    • 因此，我们将 (a,b,c)(a, b, c) 定义为一个 g2o 顶点。

  • 边 (Edge) 代表什么？

    • 边代表了变量之间的 约束 或者 测量带来的误差项。
    • 我们拥有的信息是 NN 个数据点 (xi,yi)(x_i, y_i)。每一个数据点都提供了一个约束：对于给定的 xix_i，由当前估计的 (a,b,c)(a, b, c) 计算出的预测值 exp⁡(axi2+bxi+c)\exp(ax_i^2 + bx_i + c) 应该接近观测值 yiy_i。
    • 因此，每一个数据点 (xi,yi)(x_i, y_i) 都对应图中的一条边。 这条边衡量了模型预测值与实际测量值之间的误差（残差）。

  • 顶点和边如何连接？

    • 计算第 ii 个数据点的误差 ei=yi−exp⁡(axi2+bxi+c)e_i = y_i - \exp(ax_i^2 + bx_i + c) 时，我们只需要用到参数 (a,b,c)(a, b, c) （也就是那个唯一的顶点）和对应的 xix_i 值。
    • 这意味着每条边 只连接到我们定义的那个参数顶点上。
    • 因此，这些边都是一元边 (Unary Edge)。

总结思路： 我们将创建一个包含 一个顶点 (代表参数 a, b, c) 和 N 条一元边 (每条边代表一个数据点 (x, y) 带来的约束/误差) 的图。然后，让 g2o 优化这个图，调整顶点的值 (a, b, c)，使得所有边的误差平方和（考虑信息矩阵加权）最小。

3. g2o 实现步骤

根据上面的思考，我们需要在 g2o 中完成以下步骤：

3.1 定义顶点 (CurveFittingVertex)

  • 目的: 表示待优化的参数 (a,b,c)(a, b, c)。
  • 实现:
    • 继承 g2o::BaseVertex<D, T>。
    • D (维度): 参数有 a, b, c 三个，所以维度是 3。
    • T (类型): 用 Eigen 的三维向量 Eigen::Vector3d 来存储 (a,b,c)(a, b, c)。
    • 所以继承 g2o::BaseVertex<3, Eigen::Vector3d>。
    • 重写 setToOriginImpl(): 设置顶点的初始估计值。通常设为 (0, 0, 0)。
    • 重写 oplusImpl(): 定义如何将优化算法计算出的增量 update (一个包含 Δa,Δb,Δc\Delta a, \Delta b, \Delta c 的 3 维向量) 应用到当前估计值 _estimate 上。对于 (a,b,c)(a, b, c) 这种简单的向量空间变量，直接做向量加法 _estimate += update 即可。
    • read/write 函数在此例中可以留空。

3.2 定义边 (CurveFittingEdge)

  • 目的: 表示每个数据点 (xi,yi)(x_i, y_i) 带来的约束，并计算其误差。
  • 实现:
    • 继承 g2o::BaseUnaryEdge<D, E, VertexType> (因为只连接一个顶点)。
    • D (误差维度): 误差 ei=yi−exp⁡(… )e_i = y_i - \exp(\dots) 是一个标量，所以维度是 1。
    • E (测量值类型): 测量值 yiy_i 是一个 double 类型。
    • VertexType: 这条边连接的顶点类型是 CurveFittingVertex。
    • 所以继承 g2o::BaseUnaryEdge<1, double, CurveFittingVertex>。
    • 构造函数: 计算误差时需要用到 xix_i，但 xix_i 不是待优化变量，也不是全局参数。它只与这条特定的边（这个特定的数据点）相关。因此，在创建边对象时，需要将对应的 xix_i 传入并存储在边对象的成员变量 (如 _x) 中。
    • 重写 computeError(): 这是 边的核心。在此函数中：
      1. 获取所连接的顶点 (_vertices[0])。
      2. 获取顶点当前的估计值 (a,b,c)=vertex−>estimate()(a, b, c) = \text{vertex}->\text{estimate}()。
      3. 使用存储的 _x 和当前的 (a,b,c)(a, b, c) 计算预测值 ypred=exp⁡(ax2+bx+c)y_{pred} = \exp(a\textit{x}^2 + b\textit{x} + c)。
      4. 计算误差 e=_measurement−yprede = \text{\_measurement} - y_{pred}，并将结果赋给 _error(0,0)。（_measurement 存储的是观测值 yiy_i，通过 setMeasurement() 设置）。
    • linearizeOplus() (雅可比计算): 这个例子中 没有重写 这个函数。这意味着 g2o 会默认使用 自动求导 来计算误差 ee 对参数 a,b,ca, b, c 的偏导数 (∂e∂a,∂e∂b,∂e∂c)(\frac{\partial e}{\partial a}, \frac{\partial e}{\partial b}, \frac{\partial e}{\partial c})。对于简单问题，自动求导很方便；对于性能要求高的复杂问题，通常需要手动计算并重写此函数。
    • read/write 函数在此例中可以留空。

3.3 配置优化器

  • 目的: 告诉 g2o 如何求解这个优化问题。
  • 实现:
    • 选择求解器类型:
      • BlockSolver: 定义了优化变量（顶点）和误差（边）的维度。这里是 BlockSolverTraits<3, 1> (顶点维度 3, 边维度 1)。
      • LinearSolver: 定义了如何求解线性方程组 Hx=−bHx = -b。对于规模不大的问题，可以用 LinearSolverDense；对于规模较大、稀疏性较好的问题（如 SLAM），常用 LinearSolverCSparse 或 LinearSolverCholmod。本例用了 LinearSolverCSparse。
    • 选择优化算法:
      • 常用的有 OptimizationAlgorithmLevenberg (LM 算法，鲁棒性好)、OptimizationAlgorithmGaussNewton (高斯牛顿法)、OptimizationAlgorithmDogleg。本例用了 LM 算法。
    • 创建 SparseOptimizer 对象: 这是整个图优化的管理器。
    • 设置: 将选择的优化算法设置给 SparseOptimizer，可以设置 setVerbose(true) 来打印优化过程信息。

3.4 构建图

  • 目的: 将具体的顶点和边添加到优化器中。
  • 实现:
    • 添加顶点:
      1. 创建 CurveFittingVertex 对象。
      2. 设置其初始估计值 (setEstimate)，例如 (0, 0, 0)。
      3. 设置其唯一 ID (setId)。
      4. 调用 optimizer.addVertex() 将其添加到图中。
    • 添加边:
      1. 遍历所有数据点 (xi,yi)(x_i, y_i)。
      2. 为每个数据点创建一个 CurveFittingEdge 对象，传入 xix_i。
      3. 设置其唯一 ID (setId)。
      4. 连接到顶点: 调用 edge->setVertex(0, vertex_pointer)，将边的第 0 个接口连接到之前创建的顶点。
      5. 设置测量值: 调用 edge->setMeasurement(y_i)。
      6. 设置信息矩阵: 调用 edge->setInformation(...)。信息矩阵是测量噪声协方差矩阵的逆，代表了这条边的权重。如果假设噪声是高斯分布，标准差为 σ\sigma，那么对于一维误差，信息矩阵就是 1/σ21/\sigma^2。这告诉优化器：噪声越小（σ\sigma 越小，信息矩阵越大），这个测量值就越可信，优化时应该更努力地满足这个约束。
      7. 调用 optimizer.addEdge() 将边添加到图中。

3.5 执行优化

  • 目的: 启动 g2o 的优化过程。
  • 实现:
    • 调用 optimizer.initializeOptimization() 进行初始化。
    • 调用 optimizer.optimize(max_iterations) 开始迭代优化，指定最大迭代次数。

3.6 获取结果

  • 目的: 从优化后的图中提取最终的参数估计值。
  • 实现:
    • 优化完成后，顶点对象内部的 _estimate 成员变量已经被更新为最优值。
    • 直接调用 vertex->estimate() 即可获取优化后的 (a,b,c)(a, b, c)。

4. 准备工作

在编写 g2o 代码之前，需要：

  • 包含头文件: 包含 g2o 核心库、求解器、算法以及 Eigen 库等必要的头文件。
  • 准备数据: 生成或加载用于拟合的数据点 x_data 和 y_data。例子中是程序内部生成的带高斯噪声的数据。
  • 定义参数: 确定噪声标准差 w_sigma (用于生成数据和设置信息矩阵)，数据点数量 N 等。

5. 完整示例代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include <iostream>
#include <vector>
#include <cmath>

// Eigen 核心部分
#include <Eigen/Core>

// g2o 核心库
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h> // 块求解器
#include <g2o/core/sparse_optimizer.h> // 优化器主类

// g2o 优化算法
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>

// g2o 线性求解器
#include <g2o/solvers/dense/linear_solver_dense.h> // 稠密求解器
#include <g2o/solvers/csparse/linear_solver_csparse.h> // CSparse 稀疏求解器

// OpenCV (仅用于随机数生成)
#include <opencv2/core/core.hpp>

using namespace std;
using namespace g2o;

// --- 1. 定义顶点：表示待优化参数 (a, b, c) ---
// 模板参数：优化变量维度 D=3, 数据类型 T=Eigen::Vector3d
class CurveFittingVertex : public BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // Eigen 内存对齐

    CurveFittingVertex() {} // 构造函数

    // 重置函数：设置初始估计值
    virtual void setToOriginImpl() override {
        _estimate << 0, 0, 0; // 初始猜测 a=0, b=0, c=0
    }

    // 增量更新函数：如何应用优化增量
    virtual void oplusImpl(const double *update) override {
        _estimate += Eigen::Vector3d(update); // 直接向量加法
    }

    // 读写函数 (本例中可以忽略)
    virtual bool read(istream &in) override { return false; }
    virtual bool write(ostream &out) const override { return false; }
};

// --- 2. 定义边：表示每个数据点的约束和误差 ---
// 模板参数：误差维度 D=1, 测量值类型 E=double, 连接的顶点类型 CurveFittingVertex
class CurveFittingEdge : public BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // 构造函数：传入该数据点对应的 x 值
    CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

    // 计算误差函数：核心部分
    virtual void computeError() override {
        // 获取连接的顶点 (强制类型转换)
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex *>(_vertices[0]);
        // 获取顶点当前的估计值 (a, b, c)
        const Eigen::Vector3d abc = v->estimate();
        // 计算模型预测值 y_pred = exp(a*x^2 + b*x + c)
        const double prediction = std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
        // 计算误差 error = measurement - prediction
        // _measurement 是基类成员，存储观测值 y_i
        // _error 是基类成员 (1x1 矩阵)，存储计算结果
        _error(0, 0) = _measurement - prediction;
    }

    // 线性化函数 (雅可比计算)
    // 本例中未实现，g2o 将使用自动求导
    // virtual void linearizeOplus() override { ... }

    // 读写函数 (本例中可以忽略)
    virtual bool read(istream &in) override { return false; }
    virtual bool write(ostream &out) const override { return false; }

public:
    double _x; // 存储该边对应的数据点的 x 值
};

int main(int argc, char **argv) {
    // --- 数据准备 ---
    double real_a = 1.0, real_b = 2.0, real_c = 1.0; // 真实参数
    int N = 100;                                     // 数据点数量
    double w_sigma = 1.0;                            // 噪声标准差
    cv::RNG rng;                                     // OpenCV 随机数生成器

    vector<double> x_data, y_data; // 存储数据
    cout << "Generating data..." << endl;
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(real_a * x * x + real_b * x + real_c) + rng.gaussian(w_sigma));
    }

    // --- g2o 配置 ---
    cout << "Configuring g2o..." << endl;
    // 1. 定义块求解器类型 BlockSolver< BlockSolverTraits<顶点维度, 边维度> >
    typedef BlockSolver<BlockSolverTraits<3, 1>> BlockSolverType;
    // 2. 定义线性求解器类型
    typedef LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // 3. 创建线性求解器 (使用智能指针管理)
    auto linearSolver = std::make_unique<LinearSolverType>();
    // 4. 创建块求解器 (使用智能指针管理)
    auto blockSolver = std::make_unique<BlockSolverType>(std::move(linearSolver));
    // 5. 创建优化算法 (Levenberg-Marquardt)
    OptimizationAlgorithmLevenberg *algorithm = new OptimizationAlgorithmLevenberg(std::move(blockSolver));
    // 6. 创建稀疏优化器 (图模型)
    SparseOptimizer optimizer;
    // 7. 设置优化算法
    optimizer.setAlgorithm(algorithm);
    // 8. 启用详细输出
    optimizer.setVerbose(true);

    // --- 构建图 ---
    // 1. 添加顶点
    cout << "Adding vertex..." << endl;
    CurveFittingVertex *vertex = new CurveFittingVertex();
    vertex->setEstimate(Eigen::Vector3d(0, 0, 0)); // 设置初始值
    vertex->setId(0);                             // 设置 ID
    optimizer.addVertex(vertex);                  // 添加到图中

    // 2. 添加边
    cout << "Adding " << N << " edges..." << endl;
    for (int i = 0; i < N; i++) {
        CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]); // 创建边，传入 x_i
        edge->setId(i);                                          // 设置 ID
        edge->setVertex(0, vertex);                              // 连接到顶点
        edge->setMeasurement(y_data[i]);                         // 设置测量值 y_i
        // 设置信息矩阵 (1x1)，值为 1 / sigma^2
        edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma));
        optimizer.addEdge(edge);                                 // 添加到图中
    }

    // --- 执行优化 ---
    cout << "Starting optimization..." << endl;
    optimizer.initializeOptimization(); // 初始化
    optimizer.optimize(100);            // 执行优化，最多 100 次迭代

    // --- 输出结果 ---
    cout << "Optimization finished." << endl;
    Eigen::Vector3d estimated_abc = vertex->estimate(); // 获取优化后的顶点估计值
    cout << "Estimated model parameters (a, b, c): " << estimated_abc.transpose() << endl;
    cout << "Real model parameters (a, b, c):      " << real_a << " " << real_b << " " << real_c << endl;

    // 清理 (OptimizationAlgorithmLevenberg* algorithm 是 new 出来的，理论上应 delete，
    // 但 setAlgorithm 后 optimizer 会接管，通常不需要手动 delete)
    // delete algorithm; // 可能导致重复释放

    return 0;
}