C++学习笔记—g2o库—BA

1. 问题描述

我们有一个相机，它在某个未知的位置和姿态（合称位姿）下拍摄了一张照片。我们知道：

一些三维空间点的坐标 (points_3d)。
这些三维点在相机拍摄的照片上对应的二维像素坐标 (points_2d)。
相机的内部参数（焦距、主点），通常表示为一个内参矩阵 K。
相机位姿的一个初始估计值（旋转矩阵 R 和平移向量 t）。

由于三维点坐标的测量、二维点观测以及初始位姿估计都可能存在误差，直接将三维点通过初始位姿和内参投影到图像平面，得到的二维坐标通常不会精确地等于观测到的二维坐标。

目标： 同时优化调整相机的位姿 (R, t) 和 三维点的空间坐标 (points_3d)，使得所有三维点在优化后的位姿下，根据相机内参投影到图像平面上的重投影坐标与观测到的二维坐标 (points_2d) 之间的误差（重投影误差） 的总和最小。

这本质上是一个大规模的非线性最小二乘优化问题。

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

我们将 BA 问题转化为 g2o 的图模型：

顶点 (Vertex) 代表什么？
- 顶点代表需要优化的未知变量。
- 在这个问题中，未知变量有两类：
  1. 相机的位姿 (Pose): 它有 6 个自由度（3 个旋转，3 个平移）。我们将用一个顶点来表示它。
  2. 每个三维空间点的坐标 (3D Point): 每个点有 3 个自由度（x, y, z）。我们将为每一个三维点创建一个对应的顶点。
- 因此，图中将包含 1 个位姿顶点和 N 个三维点顶点 (N 是点的数量)。
边 (Edge) 代表什么？
- 边代表变量之间的约束或测量误差。
- 我们的约束来自于观测：每个三维点 $P_i$ 在图像上被观测为一个二维点 $p_i$。这个观测关系同时关联了三维点 $P_i$ 的坐标和相机的位姿。
- 因此，每一个观测 $(P_i, p_i)$ 都对应图中的一条边。 这条边表达的是：根据当前估计的相机位姿和当前估计的三维点 $P_i$ 坐标，计算出的 $P_i$ 的重投影坐标 $p’_{i}$，与实际观测到的 $p_i$ 之间的误差 $e_i = p_i - p’_{i}$。
顶点和边如何连接？
- 计算第 $i$ 个观测的重投影误差 $e_i$ 时，我们需要用到：
  1. 相机的位姿（位姿顶点）。
  2. 第 $i$ 个三维点的坐标（第 $i$ 个点顶点）。
  3. 相机的内参 $K$（这个不是变量，而是已知参数）。
- 这意味着每条边都需要连接一个位姿顶点和一个对应的三维点顶点。
- 因此，这些边都是二元边 (Binary Edge)。
相机内参 K 如何处理？
- 在这个例子中，相机内参 $K$ 是已知且固定的，我们不优化它。
- 但是，在计算重投影误差（边的工作）时又必须用到它。
- g2o 提供了一种参数 (Parameter) 机制来处理这种情况。我们可以将相机内参封装成一个 g2o::CameraParameters 对象，并将其添加到优化器中。边在计算误差时可以访问这些参数。

总结思路： 我们将创建一个图，包含：

1 个 VertexSE3Expmap 类型的顶点（表示相机位姿）。
N 个 VertexPointXYZ 类型的顶点（表示 N 个三维点）。
1 个 CameraParameters 类型的参数（存储相机内参 K）。
N 条 EdgeProjectXYZ2UV 类型的二元边（每条边连接位姿顶点和一个点顶点，代表一个重投影误差约束，并关联相机参数）。
然后，让 g2o 优化这个图，调整所有顶点的值（位姿和点坐标），使得所有边的重投影误差平方和（考虑信息矩阵）最小。

3. g2o 实现步骤

根据 BA 的图优化思路，代码中执行了以下操作：

3.1 定义/选择顶点类型

相机位姿顶点:
- g2o 提供了预定义的 g2o::VertexSE3Expmap。
- 它内部使用 SE3Quat (包含 Eigen 的四元数和平移向量) 存储位姿估计值。
- 关键在于它的 oplusImpl 使用 李代数 se(3) 进行增量更新（通过指数映射），这对于表示旋转非常自然且避免了万向锁等问题。维度是 6。
三维点顶点:
- g2o 提供了预定义的 g2o::VertexPointXYZ。
- 它内部使用 Eigen::Vector3d 存储点的 (x, y, z) 坐标。维度是 3。
- 它的 oplusImpl 是简单的向量加法。

3.2 定义/选择边类型

重投影误差边:
- g2o 提供了预定义的 g2o::EdgeProjectXYZ2UV。
- 它是一个二元边，模板参数为 <2, Eigen::Vector2d, g2o::VertexPointXYZ, g2o::VertexSE3Expmap>，表示：
  - 误差维度 D=2 (误差是二维像素向量 $(u_{err}, v_{err})$)。
  - 测量值类型 E=Eigen::Vector2d (存储观测到的像素坐标 $(u, v)$)。
  - 连接的第一个顶点类型是 VertexPointXYZ (索引 0)。
  - 连接的第二个顶点类型是 VertexSE3Expmap (索引 1)。
- 这个类内部已经实现了 computeError() 和 linearizeOplus()：
  - computeError(): 获取位姿和点的当前估计值，获取相机参数，执行”世界坐标 -> 相机坐标 -> 像素坐标”的投影变换，计算预测像素坐标，然后用 _measurement (观测像素坐标) 减去预测坐标，得到 _error。
  - linearizeOplus(): 计算 _error (2维) 相对于 VertexPointXYZ (3维) 的雅可比矩阵 (2x3) 和相对于 VertexSE3Expmap (6维李代数) 的雅可比矩阵 (2x6)。这是 BA 的核心数学推导，g2o 已经帮你做好了。

3.3 定义参数

相机内参:
- 使用 g2o::CameraParameters 类。
- 需要用相机的焦距 $f_x$ (假设 $f_y=f_x$) 和主点 $(c_x, c_y)$ 来构造它。
- 它需要设置一个 ID，并使用 optimizer.addParameter() 添加到优化器中。

3.4 配置优化器

目的: 设置求解策略。
实现:
- 块求解器: BlockSolver< BlockSolverTraits<6, 3> >。这里的 <6, 3> 通常与 BA 中涉及的主要变量维度（位姿 6D，点 3D）相关，尤其是在使用 Schur 消元时。
- 线性求解器: LinearSolverCSparse。BA 问题通常规模较大但具有稀疏性（一个点只会被少数几个位姿看到），稀疏求解器是必要的。
- 优化算法: OptimizationAlgorithmLevenberg (LM 算法) 是 BA 的常用选择，因为它结合了高斯牛顿法和梯度下降法的优点，比较鲁棒。
- 创建 SparseOptimizer 并设置算法和 verbose 模式。

3.5 构建图

目的: 将具体的顶点、边和参数加入优化器。
实现:
- 添加位姿顶点:
  1. 创建 VertexSE3Expmap 对象。
  2. 从输入的初始 R 和 t 创建 g2o::SE3Quat 对象，并用 setEstimate() 设置初始位姿。
  3. 设置 ID (例如 0)。
  4. 添加到优化器 optimizer.addVertex()。
- 添加三维点顶点:
  1. 遍历输入的 points_3d。
  2. 为每个点创建一个 VertexPointXYZ 对象。
  3. 用 setEstimate() 设置其初始坐标。
  4. 设置唯一的 ID (例如从 1 开始递增)。
  5. point->setMarginalized(true);: 非常重要！ 这告诉 g2o 在求解线性方程 $Hx = -b$ 时，可以将这些点顶点对应的变量边缘化 (Marginalize Out)。这利用了 BA 问题的特殊结构（点只与少数位姿相关），通过 Schur 消元 (Schur Complement) 技巧，可以大大减小求解的线性系统的规模（只需求解与位姿相关的部分），从而显著提高效率。
  6. 添加到优化器 optimizer.addVertex()。
- 添加相机参数:
  1. 从内参矩阵 K 提取 $f_x, c_x, c_y$。
  2. 创建 CameraParameters 对象。
  3. 设置 ID (例如 0)。
  4. 添加到优化器 optimizer.addParameter()。
- 添加边:
  1. 遍历输入的 points_2d (需要与 points_3d 一一对应)。
  2. 为每个观测创建一个 EdgeProjectXYZ2UV 对象。
  3. 设置唯一的 ID (例如从 1 开始递增，与点 ID 对应)。
  4. 连接顶点:
    - edge->setVertex(0, ...) 连接到对应的点顶点。注意代码中使用了 optimizer.vertex(index) 通过 ID 获取顶点指针，并用 dynamic_cast 转换类型。
    - edge->setVertex(1, pose) 连接到位姿顶点。
  5. 设置测量值: edge->setMeasurement(Eigen::Vector2d(p.x, p.y))，传入观测到的像素坐标。
  6. 关联参数: edge->setParameterId(0, 0)。第一个 0 表示这是第 0 个参数集（CameraParameters），第二个 0 是该参数在优化器中的 ID。
  7. 设置信息矩阵: edge->setInformation(Eigen::Matrix2d::Identity())。这里简单地使用了单位矩阵，表示假设所有像素观测的噪声是各向同性的，且方差为 1。在实际应用中，可以根据像素噪声的估计来设置更精确的信息矩阵（例如，$\frac{1}{\sigma^2} \mathbf{I}$，其中 $\sigma$ 是像素噪声标准差）。
  8. 添加到优化器 optimizer.addEdge()。

3.6 执行优化

调用 optimizer.initializeOptimization()。
调用 optimizer.optimize(num_iterations)。

3.7 获取结果

优化完成后，位姿顶点 pose 内部的 _estimate 已经被更新。
通过 pose->estimate() 获取优化后的 g2o::SE3Quat，可以将其转换回旋转矩阵 R 和平移向量 t (代码中直接输出了 Isometry3d 形式的变换矩阵)。
如果需要，也可以遍历所有点顶点，获取优化后的三维点坐标。

4. 准备工作

包含头文件: g2o 核心库、顶点类型 (VertexSE3Expmap, VertexPointXYZ)、边类型 (EdgeProjectXYZ2UV)、参数类型 (CameraParameters)、求解器、算法、Eigen 等。
准备输入数据:
- std::vector<cv::Point3f> points_3d: 三维点坐标。
- std::vector<cv::Point2f> points_2d: 对应的二维观测点坐标。
- cv::Mat K: 相机内参矩阵。
- cv::Mat R, cv::Mat t: 相机位姿的初始估计。
数据类型转换: 需要将 OpenCV 的 cv::Mat (R, t) 和 cv::Point 转换为 g2o/Eigen 使用的 Eigen::Matrix3d, Eigen::Vector3d, Eigen::Vector2d 等。

5. 完整示例代码 (带注释)

#include <iostream>
#include <vector>
#include <cmath>

// Eigen 核心库
#include <Eigen/Core>
#include <Eigen/Geometry> // 用于 Isometry3d 等

// g2o 核心库
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h> // 二元边基类

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

// g2o 类型定义 (常用的顶点和边)
#include <g2o/types/sba/types_sba.h> // 包含 VertexSE3Expmap, VertexPointXYZ, EdgeProjectXYZ2UV, CameraParameters 等
#include <g2o/types/slam3d/se3quat.h> // SE3Quat 定义

// OpenCV (用于数据结构 Point3f, Point2f, Mat)
#include <opencv2/core/core.hpp>

using namespace std;
using namespace g2o;

/**
 * @brief 使用 g2o 进行 Bundle Adjustment (优化单个相机位姿和多个 3D 点)
 *
 * @param points_3d 输入的三维点坐标 (世界坐标系)
 * @param points_2d 对应的二维像素观测坐标
 * @param K         相机内参矩阵 (3x3)
 * @param R         相机旋转矩阵的初始估计 (3x3)
 * @param t         相机平移向量的初始估计 (3x1)
 * @note            函数会直接修改输入的 R 和 t 为优化后的结果 (虽然本示例代码没有显式传回，而是打印)
 */
void bundleAdjustment(const std::vector<cv::Point3f> points_3d,
                      const std::vector<cv::Point2f> points_2d,
                      const cv::Mat &K, cv::Mat &R, cv::Mat &t) { // R, t 是引用，理论上可以修改后传出

    // --- 1. 配置 g2o 优化器 ---
    cout << "Configuring g2o..." << endl;
    // 定义块求解器类型: BlockSolver< BlockSolverTraits<PoseDim, LandmarkDim> >
    // PoseDim=6 (SE3), LandmarkDim=3 (XYZ)
    typedef BlockSolver<BlockSolverTraits<6, 3>> BlockSolverType;
    // 定义线性求解器类型: 使用 CSparse 处理稀疏矩阵
    typedef LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // 创建线性求解器
    auto linearSolver = std::make_unique<LinearSolverType>();
    // 创建块求解器
    auto blockSolver = std::make_unique<BlockSolverType>(std::move(linearSolver));
    // 创建优化算法: Levenberg-Marquardt
    OptimizationAlgorithmLevenberg *algorithm = new OptimizationAlgorithmLevenberg(std::move(blockSolver));
    // 创建稀疏优化器 (图模型)
    SparseOptimizer optimizer;
    // 设置优化算法
    optimizer.setAlgorithm(algorithm);
    // 启用详细输出
    optimizer.setVerbose(true);

    // --- 2. 添加顶点 ---
    // a. 添加相机位姿顶点 (VertexSE3Expmap)
    cout << "Adding pose vertex..." << endl;
    VertexSE3Expmap *pose_vertex = new VertexSE3Expmap();
    pose_vertex->setId(0); // 顶点 ID 设为 0

    // 将输入的 R, t (cv::Mat) 转换为 g2o::SE3Quat 用于设置初始估计
    Eigen::Matrix3d R_mat;
    R_mat << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
             R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
             R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    Eigen::Vector3d t_vec(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0));
    pose_vertex->setEstimate(SE3Quat(R_mat, t_vec)); // 设置初始位姿估计
    optimizer.addVertex(pose_vertex); // 添加到优化器

    // b. 添加三维点顶点 (VertexPointXYZ)
    cout << "Adding " << points_3d.size() << " point vertices..." << endl;
    int point_index = 1; // 点顶点的 ID 从 1 开始
    for (const cv::Point3f p : points_3d) {
        VertexPointXYZ *point_vertex = new VertexPointXYZ();
        point_vertex->setId(point_index++); // 设置唯一 ID
        point_vertex->setEstimate(Eigen::Vector3d(p.x, p.y, p.z)); // 设置初始坐标估计
        // **关键：边缘化点**。告诉优化器在求解时使用 Schur 消元，提高效率
        point_vertex->setMarginalized(true);
        optimizer.addVertex(point_vertex); // 添加到优化器
    }

    // --- 3. 添加相机参数 ---
    cout << "Adding camera parameters..." << endl;
    // 从内参矩阵 K 提取焦距 fx 和主点 cx, cy
    double fx = K.at<double>(0, 0);
    double cx = K.at<double>(0, 2);
    double cy = K.at<double>(1, 2);
    // 创建 CameraParameters 对象 (假设 fy=fx, baseline=0)
    CameraParameters *camera = new CameraParameters(fx, Eigen::Vector2d(cx, cy), 0);
    camera->setId(0); // 参数 ID 设为 0
    optimizer.addParameter(camera); // 添加到优化器

    // --- 4. 添加边 (重投影误差) ---
    cout << "Adding " << points_2d.size() << " edges..." << endl;
    int edge_index = 1; // 边的 ID 也从 1 开始 (可以与点 ID 对应)
    for (const cv::Point2f p : points_2d) {
        EdgeProjectXYZ2UV *edge = new EdgeProjectXYZ2UV();
        edge->setId(edge_index); // 设置唯一 ID

        // **连接顶点:**
        // 边需要连接两个顶点：点顶点 (索引 0) 和位姿顶点 (索引 1)
        // 通过 ID 在优化器中查找对应的点顶点指针
        Vertex *point_v = optimizer.vertex(edge_index); // 假设边的 ID 与其对应的点顶点 ID 相同
        if (!point_v) {
             cerr << "Error: Point vertex with ID " << edge_index << " not found!" << endl;
             continue; // 或者进行错误处理
        }
        // 使用 dynamic_cast 进行类型安全的转换
        edge->setVertex(0, dynamic_cast<VertexPointXYZ *>(point_v));
        edge->setVertex(1, pose_vertex); // 连接到位姿顶点

        // **设置测量值:** 观测到的像素坐标 (u, v)
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));

        // **关联相机参数:**
        // 第一个 0: 表示这是第 0 组参数 (CameraParameters)
        // 第二个 0: 表示这组参数在优化器中的 ID 是 0
        edge->setParameterId(0, 0);

        // **设置信息矩阵:** 协方差矩阵的逆，表示测量的不确定性
        // 这里用单位矩阵，表示假设像素噪声是各向同性且方差为 1
        edge->setInformation(Eigen::Matrix2d::Identity());

        optimizer.addEdge(edge); // 添加到优化器
        edge_index++;
    }

    // --- 5. 执行优化 ---
    cout << "Starting optimization..." << endl;
    optimizer.initializeOptimization(); // 初始化
    optimizer.optimize(10);            // 执行优化，这里只迭代 10 次作为示例

    // --- 6. 获取/输出结果 ---
    cout << "Optimization finished." << endl;
    // 从位姿顶点获取优化后的估计值 (SE3Quat 类型)
    SE3Quat optimized_pose = pose_vertex->estimate();
    // 将优化后的位姿转换为 Eigen::Isometry3d (方便表示为 4x4 变换矩阵)
    Eigen::Isometry3d optimized_T(optimized_pose);
    cout << "Optimized Transformation Matrix (T_world_camera): " << endl << optimized_T.matrix() << endl;

    // 如果需要，可以将 optimized_T 分解回 R 和 t 并更新输入的 cv::Mat
    // Eigen::Matrix3d optimized_R = optimized_T.rotation();
    // Eigen::Vector3d optimized_t = optimized_T.translation();
    // ... (代码将 Eigen 矩阵/向量写回 cv::Mat R, t) ...

    // 注意：本示例没有显式清理 new 出来的顶点、边、参数和算法对象。
    // 在实际应用中，如果优化器析构时没有自动管理这些资源，可能需要手动 delete。
    // 但通常 g2o 的 SparseOptimizer 会接管所有权。
}

// --- 示例调用 (需要补充完整) ---
/*
int main() {
    // 1. 准备 points_3d, points_2d, K, R_initial, t_initial 数据
    std::vector<cv::Point3f> points_3d = { ... };
    std::vector<cv::Point2f> points_2d = { ... };
    cv::Mat K = (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    cv::Mat R_initial = ...; // 初始旋转
    cv::Mat t_initial = ...; // 初始平移

    // 2. 调用 BA 函数
    bundleAdjustment(points_3d, points_2d, K, R_initial, t_initial);

    // 3. (可选) 使用优化后的 R_initial, t_initial
    // 注意：上面的函数示例仅打印结果，未修改 R_initial, t_initial

    return 0;
}
*/