Pose Optimization #

Pose Optimization 是 ORB-SLAM2 中用于优化单帧位姿的函数。通过最小化当前帧特征点与地图点之间的重投影误差,该函数可以有效地优化当前帧的位姿。

主要步骤 #

1. 构建优化器 #

第一步函数构建了一个 g2o 优化器,并设置了相应的求解器和优化算法。 (g2o::VertexSE3Expmap) 和 Levenberg-Marquardt 。

2. 添加顶点 #

函数将当前帧的位姿添加到优化器中作为一个顶点,其 ID 为 0。注意这里的位姿是可以优化的,即没有被固定。

3. 添加边 #

接下来,函数遍历当前帧中的所有地图点,为每个有效的地图点构建一条误差边:

• 如果是单目观测,构建一条 g2o::EdgeSE3ProjectXYZOnlyPose 类型的边。
• 如果是双目观测,构建一条 g2o::EdgeStereoSE3ProjectXYZOnlyPose 类型的边。

每条边都连接了位姿顶点和一个先验的 3D 点 (地图点),其误差函数为重投影误差。边的信息矩阵根据特征点所在的尺度 (invSigma2) 进行设置。同时,为了提高鲁棒性,函数还为每条边设置了一个 Huber 核函数。

4. 多次优化与外点剔除 #

在构建完误差边之后,函数会进行 4 次优化。在每次优化之后,函数会根据卡方检验的结果将误差较大的边标记为外点,在下一次优化时不再考虑这些外点。这样可以逐步剔除错误的匹配,提高优化的精度。

5. 恢复优化后的位姿 #

函数从优化器中恢复优化后的位姿,并用其更新当前帧的位姿。函数返回的是内点的数量,作为优化质量的衡量。