ORB-SLAM2外点剔除PnPsolver

### ORB-SLAM2 中 PnP Solver 的外点剔除方法 ORB-SLAM2 是一种基于视觉的同时定位与建图（SLAM）系统，其核心之一是利用 Perspective-n-Point (PnP) 算法估计相机姿态。然而，在实际应用中，由于噪声、遮挡或其他因素的存在，可能会引入大量外点（Outliers）。为了提升系统的鲁棒性和精度，ORB-SLAM2 使用 RANSAC 算法对外点进行有效剔除。 #### 1. 基本原理 RANSAC（Random Sample Consensus, 随机抽样一致性算法）是一种常用的迭代方法，用于从一组含有外点的数据中拟合模型[^2]。具体来说，它通过随机选取少量数据点构建假设模型，并验证其余数据点对该模型的一致性。最终保留一致性强的内点集合并重新优化模型参数。 在 ORB-SLAM2 的 `PnPSolver` 类中，RANSAC 被用来剔除外点，从而提高 PnP 解算器的准确性。以下是其实现的关键部分： #### 2. 实现细节 ##### （1）输入准备 `PnPSolver::SolvePnP()` 函数接收两组对应关系作为输入： - 当前帧中的二维特征点位置； - 对应的地图点三维坐标。 这些对应关系通常由词袋模型筛选后的匹配结果提供[^3]。 ##### （2）初始化 RANSAC 参数 在调用 RANSAC 进行外点剔除之前，需要设置一些关键参数： - **最大迭代次数**：控制算法运行时间。 - **阈值**：定义一个点被认为是内点的最大允许重投影误差。 - **最小内点数**：当满足此条件时认为找到了有效的解。 ##### （3）执行 RANSAC 循环 每次循环的主要步骤如下： - 随机抽取一定数量的点对（通常是三个或四个），形成一个小样本集。 - 利用 OpenCV 提供的 `solvePnP()` 函数计算相机的姿态 $ T_{cw} $ 和旋转矩阵 $ \mathbf{R} $ 及平移向量 $ \mathbf{t} $。 - 将剩余的所有点代入当前模型，计算它们的重投影误差： $$ e_i = \| (\mathbf{x}_i^{proj}, \mathbf{y}_i^{proj}) - (\mathbf{x}_i^{obs}, \mathbf{y}_i^{obs}) \| $$ 如果某个点的误差小于设定的阈值，则将其视为内点。 - 统计内点的数量并与历史最佳记录比较，更新最优模型及其对应的内点集合。 ##### （4）优化最终模型 经过多次迭代后，选出具有最多内点的模型，并使用所有内点再次调用 `cv::solvePnP()` 或其他更精确的方法（如 EPnP）进一步细化相机姿态估计。 #### 3. 关键代码片段 以下是从源码中提取的部分逻辑展示如何实现上述功能： ```cpp // 初始化变量 std::vector<bool> vbInliers; int nIterations = 0; while(nIterations < maxIterations){ // Step 1: Randomly select a minimal set of correspondences. std::vector<size_t> vSelectedIndices = SelectMinimalSet(); // Step 2: Compute pose with the selected points using solvePnP(). cv::Mat rvec, tvec; bool bSuccess = SolveSmallSet(vSelectedIndices, rvec, tvec); if(!bSuccess || CheckDegeneracy(rvec)){ continue; // Skip degenerate cases or failures. } // Step 3: Test all observations against this model and count inliers. int nCurrentInliers = CountInliers(rvec, tvec); if(nCurrentInliers > bestInliersCount){ bestPose = PoseFromRT(rvec, tvec); // Update best solution so far. bestInliersCount = nCurrentInliers; } } if(bestInliersCount >= minRequiredInliers){ RefineWithAllInliers(); // Final refinement step. } ``` #### 4. 总结 通过结合 RANSAC 算法和 PnP 技术，ORB-SLAM2 成功实现了对外点的有效剔除，显著提升了系统在复杂环境下的稳定性和可靠性。这种方法不仅适用于单目 SLAM 场景，还可以扩展至双目或多传感器融合场景下。 ---