这个例子解决的是一个常见的 3D 重建或配准问题：我们有两组对应的三维点云（或者说，已知点之间的匹配关系），并且已经有了一个初始的、可能不太精确的相对位姿估计（旋转 R 和平移 t），我们希望通过优化来获得一个更精确的相对位姿。

与 Bundle Adjustment 的关键区别：

  • BA 通常处理的是 3D 点到 2D 图像点的投影关系。
  • 这个例子处理的是 3D 点到 3D 点 的直接变换关系。
  • 这个特定的例子只优化位姿，而不优化三维点的坐标 (尽管也可以扩展为同时优化点坐标)。

1. 问题描述

假设我们有两组三维点：

  • pts1: 第一组三维点的坐标（在坐标系 1 中）。
  • pts2: 第二组三维点的坐标（在坐标系 2 中）。

我们知道 pts1[i] 和 pts2[i] 是对应的点。我们还有一个从坐标系 2 变换到坐标系 1 的初始位姿估计 $T_{12} = (R, t)$。

目标是：优化位姿 $T_{12}$，使得将 pts2 中的每个点 $\mathbf{p}_{2,i}$ 通过优化后的位姿 $T_{12}$ 变换到坐标系 1 后，得到的结果 $T_{12} \mathbf{p}_{2,i}$ 与其对应的点 $\mathbf{p}_{1,i}$ 之间的距离（误差） 的总和最小。

误差项可以定义为： $\mathbf{e}_i = \mathbf{p}_{1,i} - T_{12} \mathbf{p}_{2,i}$

我们要最小化 $\sum_i ||\mathbf{e}_i||^2$。

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

  • 顶点 (Vertex) 代表什么？

    • 未知变量是待优化的相对位姿 $T_{12}$。
    • 因此，我们将 $T_{12}$ 定义为一个 g2o::VertexSE3Expmap 顶点。 只需要一个顶点。

  • 边 (Edge) 代表什么？

    • 约束来自于每一对匹配点 $(\mathbf{p}_{1,i}, \mathbf{p}_{2,i})$。
    • 每一对点都要求：变换后的点 $T_{12} \mathbf{p}_{2,i}$ 应该接近目标点 $\mathbf{p}_{1,i}$。
    • 因此，每一对匹配点 $(\mathbf{p}_{1,i}, \mathbf{p}_{2,i})$ 都对应图中的一条边。 这条边衡量了 $\mathbf{p}_{1,i}$ 和 $T_{12} \mathbf{p}_{2,i}$ 之间的三维向量误差。

  • 顶点和边如何连接？

    • 计算第 $i$ 对点的误差 $\mathbf{e}_i = \mathbf{p}_{1,i} - T_{12} \mathbf{p}_{2,i}$ 时，我们只用到一个待优化的变量——位姿 $T_{12}$。点 $\mathbf{p}_{1,i}$ 和 $\mathbf{p}_{2,i}$ 的坐标在这个例子中是固定的。
    • 因此，每条边只连接到那个唯一的位姿顶点上，是 一元边 (Unary Edge)。

  • 点坐标如何处理？

    • $\mathbf{p}_{1,i}$：可以看作是这条边的 测量值 (Measurement)，即我们期望变换后的点应该到达的目标位置。
    • $\mathbf{p}_{2,i}$：它是计算误差时需要用到的固定数据，它不是测量值，也不是待优化变量。它需要与边实例关联起来。

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

  • 1 个 VertexSE3Expmap 顶点 (表示待优化的位姿 $T_{12}$)。
  • N 条 自定义的一元边 (每条边对应一对匹配点 $(\mathbf{p}_{1,i}, \mathbf{p}_{2,i})$)。
    • 每条边存储 $\mathbf{p}_{2,i}$ 作为固定数据。
    • 每条边以 $\mathbf{p}_{1,i}$ 作为测量值。
    • 每条边计算误差 $\mathbf{e}_i = \mathbf{p}_{1,i} - T_{12} \mathbf{p}_{2,i}$。
      让 g2o 优化这个图，调整位姿顶点的值，使得所有边的误差平方和（考虑信息矩阵）最小。

3. g2o 实现步骤

3.1 自定义边的类型 (EdgeProjectXYZRGBDPoseOnly)

  • 原因: g2o 没有预定义完全符合 $\mathbf{e}_i = \mathbf{p}_{1,i} - T \mathbf{p}_{2,i}$ 这种形式误差的一元边。我们需要自己创建一个。
  • 实现:
    • 继承: g2o::BaseUnaryEdge<D, E, VertexType>。
      • D (误差维度): 误差是 3D 向量差，D = 3。
      • E (测量值类型): 测量值是目标点 $\mathbf{p}_{1,i}$，类型为 Eigen::Vector3d。
      • VertexType: 连接的顶点是位姿，类型为 g2o::VertexSE3Expmap。
      • 所以继承 g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>。
    • 构造函数: 需要传入并存储固定的点坐标 $\mathbf{p}_{2,i}$。代码中将其命名为 _point。
      1
      EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point) : _point(point) {}
    • 重写 computeError():
      1. 获取位姿顶点 pose = _vertices[0]。
      2. 获取当前位姿估计 T = pose->estimate()。
      3. 使用当前位姿 T 变换存储的固定点 _point (即 $\mathbf{p}_{2,i}$): transformed_point = T.map(_point)。
      4. 计算误差：_error = _measurement - transformed_point。其中 _measurement 存储的是目标点 $\mathbf{p}_{1,i}$。
    • 重写 linearizeOplus() (计算雅可比):
      1. 获取当前位姿 T 和变换后的点 xyz_trans = T.map(_point)。
      2. 计算误差 $\mathbf{e} = \mathbf{p}_1 - T \mathbf{p}_2$ 对位姿顶点（在 se(3) 李代数上的 6D 扰动 $\delta \boldsymbol{\xi} = (\delta \omega_x, \delta \omega_y, \delta \omega_z, \delta t_x, \delta t_y, \delta t_z)^T$）的雅可比矩阵 $\frac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}}$。
      3. 根据链式法则和 SE(3) 的性质，可以推导出（假设扰动是左乘的）：
        $$\frac{\partial \mathbf{e}}{\partial \delta \boldsymbol{\xi}} = - \frac{\partial (T \mathbf{p}_2)}{\partial \delta \boldsymbol{\xi}} = - \begin{pmatrix} -[\mathbf{p}’]_\times & \mathbf{I} \end{pmatrix} = \begin{pmatrix} [\mathbf{p}’]_\times & -\mathbf{I} \end{pmatrix}$$
        其中 $\mathbf{p}’ = T \mathbf{p}_2 = \text{xyz_trans}$。
      4. 将这个 3x6 的雅可比矩阵赋值给 _jacobianOplusXi (因为是一元边，只有 Xi)。代码中直接给出了展开形式。
    • read/write 函数在此例中留空。

3.2 顶点类型

  • 使用 g2o 预定义的 g2o::VertexSE3Expmap 来表示待优化的位姿。

3.3 配置优化器

  • 与 BA 示例类似，配置 BlockSolver (这里用了 BlockSolverTraits<6, 3>)、LinearSolverCSparse 和 OptimizationAlgorithmLevenberg。
  • 创建 SparseOptimizer 并设置算法和 verbose。

3.4 构建图

  • 添加顶点:
    1. 创建 VertexSE3Expmap 对象。
    2. 设置 ID (0)。
    3. 设置初始位姿估计 (setEstimate)。注意：代码中给了单位阵和零向量作为初始值，但在实际应用中，应该传入通过其他方法（如 RANSAC+SVD 或 粗略 ICP）得到的初始 R 和 t。
    4. 添加到优化器。
  • 添加边:
    1. 遍历所有匹配点对 (pts1[i], pts2[i])。
    2. 创建自定义的 EdgeProjectXYZRGBDPoseOnly 对象，将 pts2[i] (转换成 Eigen::Vector3d) 传入构造函数。
    3. 设置边的唯一 ID (从 1 开始)。
    4. 连接到顶点: edge->setVertex(0, pose)，连接到唯一的位姿顶点。
    5. 设置测量值: edge->setMeasurement(...)，传入目标点 pts1[i] (转换成 Eigen::Vector3d)。
    6. 设置信息矩阵: edge->setInformation(Eigen::Matrix3d::Identity() * 1e4)。这里使用了单位矩阵乘以一个较大的数 1e4。这意味着我们对所有的点对匹配给予了很高的信任度（权重），假设它们的误差都应该很小。在实际应用中，可以根据匹配质量或点的不确定性来设置不同的信息矩阵。
    7. 添加到优化器。

3.5 执行优化

  • 调用 optimizer.initializeOptimization()。
  • 调用 optimizer.optimize(num_iterations) (代码中是 10 次)。

3.6 获取结果

  • 从位姿顶点 pose 获取优化后的估计值 pose->estimate()。
  • 将其转换为 Eigen::Isometry3d 并输出变换矩阵。

4. 准备工作

  • 包含头文件: g2o 核心库、求解器、算法、VertexSE3Expmap、BaseUnaryEdge、Eigen 等。
  • 准备输入数据:
    • std::vector<cv::Point3f> pts1: 目标点云坐标。
    • std::vector<cv::Point3f> pts2: 源点云坐标（待变换的点）。注意 pts1 和 pts2 必须是严格对应的。
    • cv::Mat R, cv::Mat t: 初始的位姿估计（尽管代码中 bundleAdjustment 函数内部没有使用传入的 R, t 来设置初始值，而是用了单位阵，但在实际调用时应该用它们来设置 pose->setEstimate）。
  • 数据类型转换: 将 cv::Point3f 转换为 Eigen::Vector3d。

5. 关键代码片段 (带注释)

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

// Eigen 核心库
#include <Eigen/Core>
#include <Eigen/Geometry>

// 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_unary_edge.h> // 一元边基类

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

// g2o 类型定义
#include <g2o/types/slam3d/vertex_se3_expmap.h> // VertexSE3Expmap
#include <g2o/types/slam3d/se3quat.h>          // SE3Quat

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

using namespace std;
using namespace g2o;

// --- 1. 自定义边：用于 3D-3D 点对的位姿优化 ---
// 模板参数：误差维度 D=3, 测量值类型 E=Eigen::Vector3d, 连接顶点类型 VertexSE3Expmap
class EdgeProjectXYZRGBDPoseOnly : public BaseUnaryEdge<3, Eigen::Vector3d, VertexSE3Expmap> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    // 构造函数：传入需要被变换的点 p2 的坐标
    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

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

    // 计算误差函数： error = p1 - T * p2
    virtual void computeError() override {
        // 获取连接的位姿顶点
        const VertexSE3Expmap *pose = static_cast<const VertexSE3Expmap *>(_vertices[0]);
        // 计算变换后的点 T * p2 (其中 p2 是存储在 _point 中的)
        Eigen::Vector3d transformed_point = pose->estimate().map(_point);
        // 计算误差 error = measurement - transformed_point (measurement 是 p1)
        _error = _measurement - transformed_point;
    }

    // 计算雅可比矩阵： d(error) / d(delta_xi) = [p']_x, -I
    virtual void linearizeOplus() override {
        VertexSE3Expmap *pose = static_cast<VertexSE3Expmap *>(_vertices[0]);
        SE3Quat T(pose->estimate());
        // 计算变换后的点 p' = T * p2
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        // 雅可比矩阵 _jacobianOplusXi (3x6)
        // 对应扰动 delta_xi = (omega_x, omega_y, omega_z, v_x, v_y, v_z)
        // J = [ [p']_x   -I ]
        _jacobianOplusXi(0, 0) = 0;    // d(error_x)/d(omega_x)
        _jacobianOplusXi(0, 1) = -z;   // d(error_x)/d(omega_y)
        _jacobianOplusXi(0, 2) = y;    // d(error_x)/d(omega_z)
        _jacobianOplusXi(0, 3) = -1;   // d(error_x)/d(v_x)
        _jacobianOplusXi(0, 4) = 0;    // d(error_x)/d(v_y)
        _jacobianOplusXi(0, 5) = 0;    // d(error_x)/d(v_z)

        _jacobianOplusXi(1, 0) = z;    // d(error_y)/d(omega_x)
        _jacobianOplusXi(1, 1) = 0;    // d(error_y)/d(omega_y)
        _jacobianOplusXi(1, 2) = -x;   // d(error_y)/d(omega_z)
        _jacobianOplusXi(1, 3) = 0;    // d(error_y)/d(v_x)
        _jacobianOplusXi(1, 4) = -1;   // d(error_y)/d(v_y)
        _jacobianOplusXi(1, 5) = 0;    // d(error_y)/d(v_z)

        _jacobianOplusXi(2, 0) = -y;   // d(error_z)/d(omega_x)
        _jacobianOplusXi(2, 1) = x;    // d(error_z)/d(omega_y)
        _jacobianOplusXi(2, 2) = 0;    // d(error_z)/d(omega_z)
        _jacobianOplusXi(2, 3) = 0;    // d(error_z)/d(v_x)
        _jacobianOplusXi(2, 4) = 0;    // d(error_z)/d(v_y)
        _jacobianOplusXi(2, 5) = -1;   // d(error_z)/d(v_z)
    }

protected:
    Eigen::Vector3d _point; // 存储需要被变换的点 p2 的坐标
};

// --- 2. BA 函数主体 (ICP 位姿精化) ---
void bundleAdjustment(
    const std::vector<cv::Point3f> &pts1, // 目标点云 (坐标系 1)
    const std::vector<cv::Point3f> &pts2, // 源点云 (坐标系 2)
    cv::Mat &R, cv::Mat &t) {             // 初始/优化后的位姿 (R, t 从系统 2 到系统 1)

    // --- 配置 g2o 优化器 (与之前类似) ---
    typedef BlockSolver<BlockSolverTraits<6, 3>> BlockSolverType;
    typedef LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto linearSolver = std::make_unique<LinearSolverType>();
    auto blockSolver = std::make_unique<BlockSolverType>(std::move(linearSolver));
    OptimizationAlgorithmLevenberg *algorithm = new OptimizationAlgorithmLevenberg(std::move(blockSolver));
    SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);

    // --- 添加顶点 (只有一个位姿顶点) ---
    VertexSE3Expmap *pose = new VertexSE3Expmap();
    pose->setId(0);
    // **注意:** 这里设置了单位阵初始值，实际应用中应使用传入的 R, t
    // Eigen::Matrix3d R_mat; // ... 从 cv::Mat R 转换 ...
    // Eigen::Vector3d t_vec; // ... 从 cv::Mat t 转换 ...
    // pose->setEstimate(SE3Quat(R_mat, t_vec));
    pose->setEstimate(SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0))); // 示例中使用单位阵
    optimizer.addVertex(pose);

    // --- 添加边 (每个点对一条边) ---
    int index = 1; // 边 ID 从 1 开始
    for (size_t i = 0; i < pts1.size(); i++) {
        // 创建自定义边，传入需要变换的点 pts2[i]
        EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(
            Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
        edge->setId(index);
        // 连接到唯一的位姿顶点 (ID=0)
        edge->setVertex(0, pose); // dynamic_cast 在这里可能更安全，但如果确定类型可以省略
        // 设置测量值 (目标点 pts1[i])
        edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
        // 设置信息矩阵 (权重)
        // 使用单位阵乘以较大的系数，表示对匹配点对的信任度高
        edge->setInformation(Eigen::Matrix3d::Identity() * 1e4);
        optimizer.addEdge(edge);
        index++;
    }

    // --- 执行优化 ---
    cout << "Starting optimization..." << endl;
    optimizer.initializeOptimization();
    optimizer.optimize(10); // 执行 10 次迭代

    // --- 输出结果 ---
    cout << "Optimization finished." << endl;
    SE3Quat optimized_pose = pose->estimate();
    cout << "Optimized Transformation Matrix (T_1_2): " << endl << Eigen::Isometry3d(optimized_pose).matrix() << endl;

    // (可选) 将优化后的 pose 更新回输入的 R, t
}