认真的虎ORBSLAM2源码解读(十一)：PnPsolver，PnP求解器

最新推荐文章于 2022-05-11 11:35:14 发布

认真的虎

最新推荐文章于 2022-05-11 11:35:14 发布

阅读量2.3k

点赞数

分类专栏： SLAM

本文链接：https://blog.csdn.net/liu502617169/article/details/90403283

版权

这篇博客深入解读ORBSLAM2中的PnP求解器PnPSolver，包括关键函数iterate()和Refine()的详细分析，帮助读者理解PnP问题的解决方法。

摘要由CSDN通过智能技术生成

1.前言

1.1.参考博客

一起学ORBSLAM2（9）ORBSLAM的PNP解决方案
 [PnP]PnP问题之EPnP解法

2.头文件

class PnPsolver {
   
 public:
   /**构造函数
    * @param vpMapPointMatches 保存F中的特征点与与哪些mappoint匹配，vpMapPointMatches[i]表示F中第i个特征点所指向的mappoint
    */
  PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches);

  ~PnPsolver();

  //设置参数
  void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4,
                           float th2 = 5.991);

  cv::Mat find(vector<bool> &vbInliers, int &nInliers);

  //循环nIterations次执行epnp算法
  cv::Mat iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers);

 private:

   //对于此次RANSAC计算的epnp求得的位姿，原先F中的特征点与mappoint的匹配还有哪些成立
   //更新mnInliersi
  void CheckInliers();
  //以mvbBestInliers中的点对通过epnp计算位姿而不是先前使用4个点对计算位姿
  //如果计算的结果对应的inliner超过阈值mRansacMinInliers，则返回成功
  bool Refine();

  // Functions from the original EPnP code
  void set_maximum_number_of_correspondences(const int n);
  void reset_correspondences(void);
  // 将对应的3D-2D压入到pws和us
  void add_correspondence(const double X, const double Y, const double Z