我正在尝试使用来自 KITTI 数据集的立体相机对来估计相机轨迹。该程序在某些时候使用 cv::SolvePNP() 并且对于前 1500 帧它返回了相当好的结果，但之后它就完全疯狂了。
这是我目前正在做的事情：
- 加载 2 帧，将它们称为 P 和 Q（一帧是来自立体对的左右图片）
- 使用 SIFT/SURF/ORB 获取功能（尝试了所有这些）
- 匹配 P 中的特征，以及 P.left 和 Q.left 之间的匹配
- 根据匹配，我过滤掉匹配中不存在的关键点。
- 我对帧 P 上的点进行三角测量（请注意，我已经有 KITTI 数据集提供的相机矩阵）
- 我从 Homogeneous 转换点
现在是最重要的部分，我正在尝试实现我自己的 RANSAC，它将利用 solvePNP 以便从标准 cv::solvePNP 中获得更好的 rvec 和 tvec（请注意，我尝试使用 cv::solvePNPRansac 并且它工作正常但开始一段时间后也失败了）。所以 Ransac 流程是这样的：
7.1 我随机化 3D 空间（P 上的三角点）和 Q.left 上的相应关键点
7.2 我将它们 N 个点（N 是我的 RANSAC 参数）放入 cv::SolvePNP() 以获得在 2 帧之间移动了多少相机（注意我的 dist Coefs 是零矩阵），它返回给我一些 tvec 和 rvec我使用 cv::Rodrigues() 转换为 Matrix
7.3 我连接这些向量
7.4 我尝试使用以下公式将所有三角点（不仅仅是在solvePNP中使用的点）投影回2D图像：
https://i.imgur.com/aMlZw9z.png
（请注意，我没有在左侧得到 1，而是得到了一些 x、y、z 值，因此我将 x 和 y 与 z 分开）。
7.5 我发现 P.left 上的投影点和关键点之间的差异
7.6 现在我的 RANSAC 有一些阈值，我用它来过滤异常值，而不是在 RANSAC 的下一次迭代中使用它们。
7.7 我用新的（过滤的）点重复这个过程 K 次（K 也是 RANSAC 参数）
现在这适用于 80% 的帧，一段时间后它就失败了，因为 solvePNP 收到的分数太少。发生这种情况是因为没有一个投影点实际上超过了距离阈值。所以问题出在前帧的 rvec 和 tvec 中。
真的不知道我将显示代码的哪一部分，请求并编写它。
我预计 rvec 和 tvec 会出现小错误，但错误太高了。