优化——跟踪线程仅优化位姿

1. 概述

PoseOptimization 函数是ORB-SLAM2中用于跟踪线程 (Tracking Thread) 的一个关键优化步骤。其核心目标是：

仅优化当前帧的相机位姿 ( $SE(3)$ 变换)。
不优化地图点 (MapPoints) 的三维位置，即假设地图点在世界坐标系中的位置是固定的。

此函数主要应用在跟踪过程中的运动跟踪、参考帧跟踪、地图跟踪和重定位阶段。

2. 图优化基础：顶点与边

该优化过程基于图优化理论，使用g2o库来实现。图由顶点和边构成。

2.1. 顶点 (Vertex)

定义: 代表当前帧的相机位姿，这也是本次优化的待优化变量。
表示: 通常是一个 $SE(3)$ $S E (3)$ 变换，描述了从世界坐标系 (World Frame) 到当前相机坐标系 (Camera Frame) 的旋转 ( $R$ $R$ ) 和平移 ( $t$ $t$ )。
- $T_{cw} = \begin{pmatrix} R_{cw} & t_{cw} \\ 0 & 1 \end{pmatrix}$
g2o类型: g2o::VertexSE3Expmap。使用李代数 $se(3)$ 进行优化。
特点:
- 在优化图中，通常只有一个这样的顶点（对应当前帧的位姿）。
- 其他帧的位姿或地图点在此特定优化中被视为固定，不参与优化。
- 顶点ID通常设为0，且 setFixed(false) 表明它是可优化的。

2.2. 边 (Edge)

定义: 代表一个地图点在当前帧中的投影约束。每一条边都关联一个三维地图点和当前帧的位姿。
类型:
- 一元边 (Unary Edge): 因为它只连接一个待优化的顶点（即当前帧的位姿）。地图点本身是固定的，不作为优化图中的顶点。
- g2o类型 (单目): g2o::EdgeSE3ProjectXYZOnlyPose。
- g2o类型 (双目/RGB-D): g2o::EdgeStereoSE3ProjectXYZOnlyPose。
误差定义 (核心): 边的误差是观测到的二维特征点位置与三维地图点根据当前估计位姿投影到图像上的二维位置之间的差，即重投影误差 (Reprojection Error)。

3. 误差函数详解

3.1. 单目相机 (Monocular Camera)

对于 g2o::EdgeSE3ProjectXYZOnlyPose：

误差向量维度: 2 (图像 $x, y$ 坐标的差)。
误差计算:
$e = z_{obs} - \pi(T_{cw} \cdot P_w)$ $e = z_{o b s} - π (T_{c w} \cdot P_{w})$
其中：
- $e$ : 二维误差向量。
- $z_{obs}$ : 观测到的特征点在当前帧图像上的二维像素坐标 $(u_{obs}, v_{obs})^T$ 。这是通过特征提取和匹配得到的。
- $P_w$ : 该特征点对应的三维地图点在世界坐标系下的坐标 $(X_w, Y_w, Z_w)^T$ 。这个坐标在 PoseOptimization 中是固定不变的。
- $T_{cw}$ : 当前估计的相机位姿 (从世界到相机的变换)，即图中的顶点 vSE3->estimate()。
- $\pi(\cdot)$ $π (\cdot)$ : 相机投影函数，将相机坐标系下的三维点投影到二维图像平面。
  1. 坐标变换: $P_c = T_{cw} \cdot P_w = R_{cw}P_w + t_{cw}$ $P_{c} = T_{c w} \cdot P_{w} = R_{c w} P_{w} + t_{c w}$ 。
    在代码中对应 v1->estimate().map(Xw)。
    - v1 即当前帧位姿顶点 VertexSE3Expmap*。
    - estimate() 获取当前位姿的估计值 ( $SE3Quat$ )。
    - map(Xw) 用此位姿将世界坐标 Xw 变换到相机坐标系下 $P_c = (X_c, Y_c, Z_c)^T$ 。
  2. 归一化平面投影:
    $x' = X_c / Z_c$
    $y' = Y_c / Z_c$
    在代码中对应 project2d(trans_xyz)。
  3. 像素坐标转换 (内参):
    $u_{proj} = f_x \cdot x' + c_x$
    $v_{proj} = f_y \cdot y' + c_y$
    在代码中对应 cam_project(trans_xyz) 中的后续步骤。
- 最终误差: $e = \begin{pmatrix} u_{obs} \\ v_{obs} \end{pmatrix} - \begin{pmatrix} u_{proj} \\ v_{proj} \end{pmatrix}$ 。

3.2. 双目相机或RGB-D相机 (Stereo/RGB-D Camera)

对于 g2o::EdgeStereoSE3ProjectXYZOnlyPose：

误差向量维度: 3。
观测值:
- 对于双目相机，观测值通常是左目图像的 $(u_L, v_L)$ 和右目图像中对应点的横坐标 $u_R$ 。
- $z_{obs} = (u_{L,obs}, v_{L,obs}, u_{R,obs})^T$
投影:
1. $P_c = T_{cw} \cdot P_w = (X_c, Y_c, Z_c)^T$ .
2. 左目投影 $(u_{L,proj}, v_{L,proj})$ 与单目类似：
  $u_{L,proj} = f_x \cdot (X_c / Z_c) + c_x$
  $v_{L,proj} = f_y \cdot (Y_c / Z_c) + c_y$
3. 右目横坐标投影 $u_{R,proj}$ ：
  $u_{R,proj} = u_{L,proj} - \frac{f_x \cdot b}{Z_c}$
  其中 $b$ 是双目相机的基线 (baseline)。在代码中通常用 bf 表示 $f_x \cdot b$ 。
误差计算:
$e = \begin{pmatrix} u\_{L,obs} \\ v\_{L,obs} \\ u\_{R,obs} \end{pmatrix} - \begin{pmatrix} u\_{L,proj} \\ v_{L,proj} \\ u_{R,proj} \end{pmatrix}$

3.3. 信息矩阵 (Information Matrix) $\Omega$

每条边都有一个信息矩阵 $\Omega$ ，它是误差协方差矩阵的逆。
$\Omega$ $Ω$ 反映了观测的置信度。通常与特征点所在的图像金字塔层级 (octave) 有关。
- const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
- e->setInformation(Eigen::Matrix2d::Identity() * invSigma2); (单目)
- 层级越高（图像越缩放，特征点越不稳定），invSigma2 越小，表示信息量越低，权重越小。

3.4. 鲁棒核函数 (Robust Kernel)

为了降低外点 (outliers) (即错误的特征匹配或动态物体上的点) 对优化结果的负面影响，通常会使用鲁棒核函数，如Huber核。
- g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
- rk->setDelta(deltaMono); (deltaMono 通常基于卡方分布计算，如 $\sqrt{5.991}$ 对应自由度为2，置信度95%)
- e->setRobustKernel(rk);
鲁棒核函数会调整误差较大边的权重，使其对总代价函数的影响减小。
在优化的后期迭代中，当误差普遍较小时，可能会移除鲁棒核 (e->setRobustKernel(0);)。

4. `Optimizer::PoseOptimization` 函数执行流程

Step 1: 构造g2o优化器
- 选择求解器类型，如 BlockSolver_6_3 (位姿6维，地图点3维，但这里地图点不优化)。
- 选择线性求解器，如 LinearSolverDense。
- 选择非线性优化算法，如 OptimizationAlgorithmLevenberg (Levenberg-Marquardt)。
- g2o::SparseOptimizer optimizer;
- optimizer.setAlgorithm(solver);
Step 2: 添加顶点 (Vertex)
- 将当前帧的初始位姿估计 pFrame->mTcw (转换为 SE3Quat) 作为顶点添加到优化器中。
- g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
- vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
- vSE3->setId(0);
- vSE3->setFixed(false); // 表明此顶点是待优化的
- optimizer.addVertex(vSE3);
Step 3: 添加一元边 (Unary Edges)
- 遍历当前帧 pFrame 中所有有效的地图点匹配 mvpMapPoints。
- 对于每个有效的地图点 pMP:
  - 获取观测: 特征点在当前帧归一化图像平面上的坐标 kpUn.pt.x, kpUn.pt.y。
  - 创建边:
    - 单目: g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
    - 双目/RGB-D: (类似地创建 EdgeStereoSE3ProjectXYZOnlyPose)
  - 关联顶点: e->setVertex(0, optimizer.vertex(0)); (连接到ID为0的位姿顶点)。
  - 设置测量值: e->setMeasurement(obs); (观测到的二维/三维坐标)。
  - 设置信息矩阵: 根据特征点金字塔层级 invSigma2。
  - 设置鲁棒核: (如Huber核) e->setRobustKernel(rk);
  - 设置相机内参: e->fx, e->fy, e->cx, e->cy (对于单目) 或 e->bf (对于双目)。
  - 设置地图点世界坐标: e->Xw = pMP->GetWorldPos(); (这是固定的)。
  - 添加边到优化器: optimizer.addEdge(e);
  - 记录边和对应的地图点索引，用于后续内外点判断。
Step 4: 执行优化与内外点筛选
- 进行多轮优化迭代 (例如，书中提到共4次主优化，每次主优化内部迭代10次)。
  - const int its[4] = {10, 10, 10, 10};
- 在每轮主优化开始前，可以重新初始化顶点估计值:
  - vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw)); (这里用的是进入优化前的位姿，或者上一轮优化的结果也可以)
- 初始化优化器针对特定级别的边: optimizer.initializeOptimization(0);
- 执行单轮优化: optimizer.optimize(its[it]);
- 内外点判断 (Outlier Rejection):
  - 优化后，遍历所有边，计算其卡方误差 chi2 = e->chi2() (即 $e^T \Omega e$ )。
  - if (chi2 > chi2Threshold):
    - 将此观测标记为外点: pFrame->mvbOutlier[idx] = true;
    - 在g2o中，将此外点的优化等级设为1 (e->setLevel(1);)，使其在下一轮 initializeOptimization(0) 时不被激活。
  - Else (内点):
    - 标记为内点: pFrame->mvbOutlier[idx] = false;
    - 优化等级设为0 (e->setLevel(0);)。
  - 根据内外点情况，可能在若干轮优化后移除鲁棒核 (e->setRobustKernel(0);)，因为此时大部分外点已被剔除，误差主要由高斯噪声引起。
- 如果内点数量过少 (如小于3)，则放弃优化。
Step 5: 更新位姿
- 从优化器中获取优化后的位姿估计。
- g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
- g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
- 将g2o的 $SE3Quat$ 格式转换回 cv::Mat。
- cv::Mat pose = Converter::toCvMat(SE3quat_recov);
- 更新当前帧的位姿: pFrame->SetPose(pose);

返回值: 优化后的内点数量。

5. 总结

PoseOptimization 是一个轻量级的优化过程，通过最小化地图点在当前帧的重投影误差来快速估计当前相机的精确位姿。它只调整当前帧的6自由度位姿，保持地图点不变，因此计算效率较高，非常适合实时性要求高的跟踪线程。通过迭代优化和内外点判断，可以得到一个鲁棒且精确的位姿估计。