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