C++学习笔记—g2o库—曲线拟合

1. 问题描述

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

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

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

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

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

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

• 顶点 (Vertex) 代表什么？

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

• 边 (Edge) 代表什么？

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

• 顶点和边如何连接？

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

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

3. g2o 实现步骤

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

3.1 定义顶点 (CurveFittingVertex)

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

3.2 定义边 (CurveFittingEdge)

• 目的: 表示每个数据点 $(x_i, y_i)$ 带来的约束，并计算其误差。
• 实现:
  • 继承 g2o::BaseUnaryEdge<D, E, VertexType> (因为只连接一个顶点)。
  • D (误差维度): 误差 $e_i = y_i - \exp(\dots)$ 是一个标量，所以维度是 1。
  • E (测量值类型): 测量值 $y_i$ 是一个 double 类型。
  • VertexType: 这条边连接的顶点类型是 CurveFittingVertex。
  • 所以继承 g2o::BaseUnaryEdge<1, double, CurveFittingVertex>。
  • 构造函数: 计算误差时需要用到 $x_i$，但 $x_i$ 不是待优化变量，也不是全局参数。它只与这条特定的边（这个特定的数据点）相关。因此，在创建边对象时，需要将对应的 $x_i$ 传入并存储在边对象的成员变量 (如 _x) 中。
  • 重写 computeError(): 这是 边的核心。在此函数中：
    1. 获取所连接的顶点 (_vertices[0])。
    2. 获取顶点当前的估计值 $(a, b, c) = \text{vertex}->\text{estimate}()$。
    3. 使用存储的 _x 和当前的 $(a, b, c)$ 计算预测值 $y_{pred} = \exp(a\textit{x}^2 + b\textit{x} + c)$。
    4. 计算误差 $e = \text{_measurement} - y_{pred}$，并将结果赋给 _error(0,0)。（_measurement 存储的是观测值 $y_i$，通过 setMeasurement() 设置）。
  • linearizeOplus() (雅可比计算): 这个例子中 没有重写 这个函数。这意味着 g2o 会默认使用 自动求导 来计算误差 $e$ 对参数 $a, b, 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 = -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. 遍历所有数据点 $(x_i, y_i)$。
    2. 为每个数据点创建一个 CurveFittingEdge 对象，传入 $x_i$。
    3. 设置其唯一 ID (setId)。
    4. 连接到顶点: 调用 edge->setVertex(0, vertex_pointer)，将边的第 0 个接口连接到之前创建的顶点。
    5. 设置测量值: 调用 edge->setMeasurement(y_i)。
    6. 设置信息矩阵: 调用 edge->setInformation(...)。信息矩阵是测量噪声协方差矩阵的逆，代表了这条边的权重。如果假设噪声是高斯分布，标准差为 $\sigma$，那么对于一维误差，信息矩阵就是 $1/\sigma^2$。这告诉优化器：噪声越小（$\sigma$ 越小，信息矩阵越大），这个测量值就越可信，优化时应该更努力地满足这个约束。
    7. 调用 optimizer.addEdge() 将边添加到图中。

3.5 执行优化

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

3.6 获取结果

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

4. 准备工作

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

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

5. 完整示例代码

#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;
}