闭环线程中的 Sim(3) 位姿优化

理解在检测到潜在的闭环时，系统如何优化当前关键帧 (KF1) 与闭环候选关键帧 (KF2) 之间的相对位姿。此相对位姿通过一个 Sim(3) 变换（包含旋转、平移和尺度信息）来描述。关键点在于，此优化过程仅针对 Sim(3) 位姿进行，地图点的三维坐标在此步骤中保持固定，不参与优化。

一、核心思想：图优化 (Graph Optimization)

该优化过程依赖于图优化方法，具体实现通常借助 g2o (General Graph Optimization) 框架。图优化的基本组成元素如下：

顶点 (Vertices)： 图中的节点，代表了系统中待优化的变量。在我们的场景中，主要是 Sim(3) 位姿。
边 (Edges)： 连接顶点的边，代表了这些变量之间存在的约束关系。这些约束通常以误差函数的形式表达，优化的目标是最小化所有误差的总和。

二、顶点的选择 (Vertices)

在闭环的 Sim(3) 位姿优化问题中，主要涉及以下两种类型的顶点：

待优化的 Sim(3) 位姿:
- 描述： 这是我们核心的优化目标，代表了从闭环候选关键帧 KF2 到当前关键帧 KF1 的 7 自由度相似变换 ( $Sim(3) = (s, R, t)$ )，其中 $s$ 为尺度因子， $R$ 为旋转矩阵， $t$ 为平移向量。
- g2o 类型： 在 g2o 中，这类顶点通常表示为 g2o::VertexSim3Expmap。它使用李代数 $sim(3)$ 进行参数化。
- 图示 (参考图 14-3)： 例如 g2oS12，在优化器中可能对应 optimizer.vertex(0)。
匹配的地图点 (MapPoints):
- 描述： 这些是在 KF1 和 KF2 中共同观测到的三维地图点。在 Sim(3) 位姿优化阶段，这些地图点的三维坐标被认为是准确的，因此它们是固定的 (fixed)，不参与优化。它们作为稳定的参考点，为优化 Sim(3) 位姿提供约束。
- g2o 类型： 在 g2o 中，三维点通常表示为 g2o::VertexSBAPointXYZ (SBA: Sparse Bundle Adjustment)。
- 图示 (参考图 14-3)： 例如点 P3D2c，在优化器中可能对应 optimizer.vertex(id2)，其中 id2 是该地图点在优化器中的唯一标识。

三、边的选择 (Edges)

边定义了顶点之间的约束，即地图点在 Sim(3) 变换下的投影关系。这些是二元边，因为它连接了两种类型的顶点：一个地图点顶点和一个 Sim(3) 位姿顶点。主要存在两种类型的边，对应于投影的方向：

正向投影边 (Forward Projection Edge):
- 描述： 将闭环候选关键帧 KF2 中的一个地图点（在其自身的相机坐标系下），通过待优化的 Sim(3) 位姿 (g2oS12)，变换到当前关键帧 KF1 的相机坐标系下，并进一步投影到 KF1 的图像平面上。其误差是该投影点与 KF1 中对应特征点的实际观测位置之间的差异。
- g2o 类型： g2o::EdgeSim3ProjectXYZ。
- 图示 (参考图 14-3)： 边 e12 连接了 KF2 中的地图点 P3D2c (通过 optimizer.vertex(id2)) 和 Sim(3) 位姿 g2oS12 (通过 optimizer.vertex(0)), 其测量值 (measurement) 是 KF1 中的观测 obs1。
反向投影边 (Inverse Projection Edge):
- 描述： 将当前关键帧 KF1 中的一个地图点（在其自身的相机坐标系下），通过待优化的 Sim(3) 位姿的逆变换 (g2oS12的逆，即g2oS21)，变换到闭环候选关键帧 KF2 的相机坐标系下，并进一步投影到 KF2 的图像平面上。其误差是该投影点与 KF2 中对应特征点的实际观测位置之间的差异。
- g2o 类型： g2o::EdgeInverseSim3ProjectXYZ。

四、误差的定义 (以正向投影边 `g2o::EdgeSim3ProjectXYZ` 为例)

边的核心是其误差函数，优化器会尝试最小化所有边误差的平方和。对于 Sim(3) 投影边，误差通常是重投影误差。其计算步骤如下：

获取三维点坐标：
从地图点顶点中获取其在源关键帧（例如 KF2）的相机坐标系下的三维坐标 $\mathbf{P}_{\text{c2}}$ 。在代码中如 v2->estimate()。
通过 Sim(3) 位姿进行变换：
使用当前估计的 Sim(3) 位姿 $S_{12}$ $S_{12}$ (从 KF2 到 KF1 的变换，代码中如 v1->estimate())，将三维点 $\mathbf{P}{\text{c2}}$ $P c2$ 从 KF2 的相机坐标系变换到 KF1 的相机坐标系下，得到 $\mathbf{P}{\text{c1}}$ 。
$\mathbf{P}{\text{c1}} = S{12} \cdot \mathbf{P}{\text{c2}} = s \cdot (R \cdot \mathbf{P}{\text{c2}} + \mathbf{t})$ $P c1 = S 12 \cdot P c2 = s \cdot (R \cdot P c2 + t)$
这对应代码中的 v1->estimate().map(v2->estimate())。
- map 函数内部实现: return s * (r * xyz) + t;
投影到归一化图像平面：
将变换到 KF1 相机坐标系下的三维点 $\mathbf{P}_{\text{c1}} = (X, Y, Z)^T$ $P_{c1} = (X, Y, Z)^{T}$ 投影到其归一化图像平面（ $z=1$ $z = 1$ 平面），得到二维坐标 $(u', v')^T$ $(u^{'}, v^{'})^{T}$ 。
$u' = X / Z$ $u^{'} = X / Z$
$v' = Y / Z$ $v^{'} = Y / Z$
这对应代码中的 project(transformed_point) 函数。
- project 函数内部实现: res(0) = v(0)/v(2); res(1) = v(1)/v(2);
转换为像素坐标：
使用目标关键帧 KF1 的相机内参数（焦距 $f_{x1}, f_{y1}$ $f_{x 1}, f_{y 1}$ 和主点 $c_{x1}, c_{y1}$ $c_{x 1}, c_{y 1}$ ），将归一化平面坐标 $(u', v')^T$ $(u^{'}, v^{'})^{T}$ 转换为像素坐标 $(u_p, v_p)^T$ $(u_{p}, v_{p})^{T}$ 。
$u_p = f_{x1} \cdot u' + c_{x1}$ $u_{p} = f_{x 1} \cdot u^{'} + c_{x 1}$
$v_p = f_{y1} \cdot v' + c_{y1}$ $v_{p} = f_{y 1} \cdot v^{'} + c_{y 1}$
这对应代码中的 cam_map1(projected_point) 函数。
- cam_map1 函数内部实现: res[0] = v[0]*_focal_length1[0] + _principle_point1[0]; res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
计算误差向量：
误差 $\mathbf{e}$ 是实际观测到的像素坐标 $\mathbf{z}{\text{obs}} = (u{\text{obs}}, v_{\text{obs}})^T$ （即 KF1 中匹配的特征点位置）与步骤 4 中投影得到的像素坐标 $(u_p, v_p)^T$ 之间的差值。
$\mathbf{e} = \mathbf{z}_{\text{obs}} - (u_p, v_p)^T$
这对应代码中的 _error = obs - projected_pixel_coordinates;

五、Sim(3) 位姿优化的流程 (Optimizer::OptimizeSim3 函数详解)

在 ORB-SLAM2 中，Optimizer::OptimizeSim3 函数负责执行此优化。其主要步骤如下：

步骤 1：初始化 g2o 优化器
- 创建一个 g2o::SparseOptimizer 对象。
- 配置求解器：通常选择 Levenberg-Marquardt (LM) 算法 (g2o::OptimizationAlgorithmLevenberg)。
- 配置线性求解器：例如 g2o::LinearSolverDense 或 g2o::LinearSolverCholmod，取决于问题的规模和稀疏性。
步骤 2：设置待优化的 Sim(3) 位姿作为顶点
- 创建一个 g2o::VertexSim3Expmap 类型的顶点 vSim3。
- 设置其初始估计值：使用传入的 g2oS12。
- 设置顶点 ID：通常设为 0 (vSim3->setId(0))。
- 设置顶点不固定： vSim3->setFixed(false)，因为这是我们要优化的变量。
- 处理尺度固定性：
  - vSim3->_fix_scale = bFixScale;
  - 如果传感器是单目相机 (Monocular)，bFixScale 通常为 false，尺度参数会参与优化。
  - 如果传感器是双目相机 (Stereo) 或 RGB-D 相机，bFixScale 通常为 true，尺度参数固定为 1（或由双目/RGB-D直接提供），不参与优化。
- 存储相关的相机内参（主点 _principle_point1, _principle_point2 和焦距 _focal_length1, _focal_length2）到 vSim3 顶点中，供边的误差函数计算使用。这些内参是从 KF1 和 KF2 的相机参数中获取的。
- 将此 Sim(3) 顶点添加到优化器中 (optimizer.addVertex(vSim3))。
步骤 3：设置匹配的地图点作为顶点 (固定)
- 获取 KF1 和 KF2 之间匹配的地图点列表 (vpMatches1)。
- 遍历每一对匹配的地图点 (pMP1 来自 KF1, pMP2 来自 KF2)。
- 检查有效性： 确保 pMP1 和 pMP2 都不是坏点 (!pMP1->isBad() && !pMP2->isBad())，并且 pMP2 在 KF2 中有对应的二维特征点索引 (i2 >= 0)。
- 如果有效：
  - 为 pMP1 创建顶点 (KF1 相关):
    - 创建一个 g2o::VertexSBAPointXYZ 顶点 vPoint1。
    - 获取 pMP1 的世界坐标 P3D1w，然后使用 KF1 的位姿 (R1w, t1w) 将其转换到 KF1 的相机坐标系下 P3D1c = R1w * P3D1w + t1w。
    - 设置 vPoint1 的估计值为 P3D1c。
    - 设置顶点 ID (例如 2*i + 1，以避免与 Sim(3) 顶点和其他点顶点冲突)。
    - 设置顶点固定： vPoint1->setFixed(true)。
    - 将 vPoint1 添加到优化器。
  - 为 pMP2 创建顶点 (KF2 相关):
    - 创建一个 g2o::VertexSBAPointXYZ 顶点 vPoint2。
    - 获取 pMP2 的世界坐标 P3D2w，然后使用 KF2 的位姿 (R2w, t2w) 将其转换到 KF2 的相机坐标系下 P3D2c = R2w * P3D2w + t2w。
    - 设置 vPoint2 的估计值为 P3D2c。
    - 设置顶点 ID (例如 2*(i+1)).
    - 设置顶点固定： vPoint2->setFixed(true)。
    - 将 vPoint2 添加到优化器。
- 对有效匹配进行计数 (nCorrespondences++)。
步骤 4：设置地图点投影关系作为边
对于每一组通过了步骤 3 检查的有效匹配 (pMP1, pMP2, 以及 KF1 中的观测 kpUn1 和 KF2 中的观测 kpUn2)：
- 4.1 添加正向投影边 (KF2 -> KF1):
  - 创建一个 g2o::EdgeSim3ProjectXYZ 类型的边 e12。
  - 设置连接的顶点：
    - 第一个顶点 (index 0 in edge) 是 KF2 的地图点顶点 vPoint2 (代码中用 optimizer.vertex(id2))。
    - 第二个顶点 (index 1 in edge) 是 Sim(3) 位姿顶点 vSim3 (代码中用 optimizer.vertex(0))。
  - 设置测量值 (Measurement)：使用 KF1 中对应的未畸变特征点的像素坐标 obs1 = (kpUn1.pt.x, kpUn1.pt.y)。
  - 设置信息矩阵 (Information Matrix)：通常是一个对角矩阵，其对角元素与特征点观测的确定性有关，一般取图像金字塔层级的逆方差 (invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave])。信息矩阵是协方差矩阵的逆，表示测量值的可信度。
    e12->setInformation(Eigen::Matrix2d::Identity() * invSigmaSquare1);
  - 设置鲁棒核函数 (Robust Kernel)：例如 Huber 核 (g2o::RobustKernelHuber)，以减小离群点 (outliers) 对优化结果的负面影响。设置核函数的宽度 (deltaHuber)。
  - 将边 e12 添加到优化器，并存入列表 vpEdges12。
- 4.2 添加反向投影边 (KF1 -> KF2):
  - 创建一个 g2o::EdgeInverseSim3ProjectXYZ 类型的边 e21。
  - 设置连接的顶点：
    - 第一个顶点是 KF1 的地图点顶点 vPoint1。
    - 第二个顶点是 Sim(3) 位姿顶点 vSim3。
  - 设置测量值：使用 KF2 中对应的未畸变特征点的像素坐标 obs2。
  - 设置信息矩阵（基于 KF2 中特征点的金字塔层级）。
  - 设置鲁棒核函数。
  - 将边 e21 添加到优化器，并存入列表 vpEdges21。
- 记录边的索引，用于后续的 outlier 剔除。
步骤 5：第一次 g2o 优化
- 初始化优化 (optimizer.initializeOptimization())。
- 执行固定次数的迭代优化，例如 5 次 (optimizer.optimize(5))。
步骤 6：用卡方检验剔除误差大的边 (Outlier Rejection)
- 遍历步骤 4 中添加的所有边对 (vpEdges12[i] 和 vpEdges21[i])。
- 对于每条边，计算其卡方误差 (e12->chi2() 和 e21->chi2())。卡方值是误差项经过信息矩阵加权后的平方和，表示这条边对当前模型拟合的好坏程度。
- 如果正向投影边或反向投影边的卡方误差任何一个超过预设的阈值 th2：
  - 认为这对匹配是外点 (outlier)。
  - 从优化器中移除这两条边 (optimizer.removeEdge(e12), optimizer.removeEdge(e21)).
  - 将对应的原始匹配关系 vpMatches1[idx] 标记为无效 (例如设为 NULL)。
  - 将 vpEdges12[i] 和 vpEdges21[i] 在列表中的对应位置也标记为无效。
  - 累加坏点（被剔除的匹配对）的数量 nBad++。
步骤 7：第二次 g2o 优化 (根据情况调整迭代次数)
- 根据 nBad 的数量决定迭代次数：
  - 如果 nBad > 0 (即有边被剔除)，说明初始的匹配中存在较多外点，闭环质量可能不是非常好，此时进行更多次迭代，例如 10 次 (nMoreIterations = 10)。
  - 如果 nBad == 0 (没有边被剔除)，说明初始匹配质量较高，进行较少次数迭代，例如 5 次 (nMoreIterations = 5)。
- 检查剩余有效匹配数量： 如果经过外点剔除后，剩余的有效匹配数量 (nCorrespondences - nBad) 过少（例如小于 10），则认为闭环质量太差，放弃本次优化，直接返回 0 (表示没有足够的内点支持)。
- 在剔除了外点之后，再次执行 g2o 优化 (optimizer.optimize(nMoreIterations))。
步骤 8：用优化后的结果更新 Sim(3) 位姿
- 从优化器中获取优化后的 Sim(3) 顶点：
  g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
- 用该顶点的最新估计值更新输入的 g2oS12 参数：
  g2oS12 = vSim3_recov->estimate();
- 返回内点的数量 (nCorrespondences - nBad)，这个数量可以作为 Sim(3) 优化成功与否以及闭环质量的一个指标。

六、总结图 14-3 (Sim(3) 位姿优化中的顶点和边)

当前关键帧 KF1: 我们试图将闭环候选帧对齐到的参考帧。
闭环候选关键帧 KF2: 检测到的可能与 KF1 形成闭环的帧。
g2oS12 (指向 optimizer.vertex(0)): 核心待优化的 Sim(3) 变换。它描述了如何将 KF2 的坐标系（及其关联的地图点）变换到 KF1 的坐标系。
P3D2c (指向 optimizer.vertex(id2)): 一个在 KF2 的相机坐标系下表示的三维地图点。这个点在优化过程中其坐标是固定不变的。
obs1: 地图点 P3D2c 在 KF1 图像上的实际观测到的二维像素坐标。这是边的测量值。
e12 (类型为 g2o::EdgeSim3ProjectXYZ()): 一条正向投影边。它约束了 g2oS12。该边的误差计算过程是：将 P3D2c 通过当前的 g2oS12 估计值从 KF2 坐标系变换到 KF1 坐标系，再投影到 KF1 的图像平面，然后与 obs1 比较得出重投影误差。

七、简要总结

闭环线程中的 Sim(3) 位姿优化是一个至关重要的步骤，它旨在精确估计当前关键帧与过去某个关键帧之间的相对位姿（包括尺度）。通过构建一个包含 Sim(3) 位姿和固定地图点的图模型，并利用它们之间的重投影约束，使用如图优化（g2o）的迭代方法来最小化重投影误差。这个过程还包括鲁棒的外点剔除机制，以确保优化结果的准确性和稳定性，为后续的闭环融合和全局姿态图优化奠定基础。