ORBmatacher

Posted 2020-10-16 奋斗的妮子

该类主要用于ORB特征点匹配。

public中：

1.计算两个ORB描述子之间的汉明距离：

1 static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);

　　（1）该函数是静态成员函数，为类的全部服务而不是某一个类的具体对象服务。具体用来，可以保证当前计算得到的距离是刚刚调用过该函数的最新结果。

　　（2）计算汉明距离的参考代码：http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel

2.四个重载的SearchByProjection()函数：

（1）寻找当前帧和投影地图的特征匹配点，返回匹配点数量。用于跟踪局部地图。

1 int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th=3);

寻找最佳匹配点对的原则：

a.两个特征点所属的金字塔层一致；

b.最佳匹配点对距离>mfNNratio×次优匹配点对距离。

1 // Apply ratio to second match (only if best and second are in the same scale level)
2 if(bestDist<=TH_HIGH)
3 {
4     if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
5            continue;
6 
7      F.mvpMapPoints[bestIdx]=pMP;
8      nmatches++;
9  }

 

（2）将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧。

1 int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)

a.建立旋转直方图，用于检测旋转一致性；

b.先分别计算当前帧和前一帧的旋转矩阵和平移矩阵；

c.对于前一帧的每一个地图点，通过相机模型，映射得到当前帧的像素坐标；

d.在一个窗口内搜寻特征点，窗口尺寸根据尺度进行变化。因为前面ORB是进行了高斯金字塔分层，金字塔越上层的尺寸越小，搜索窗口相应就小；

e.计算特征点之间的距离，如果距离小于设定值TH_HIGH，则认为是匹配点对；

f.应用旋转一致性再一次筛选匹配点对。

（3）将关键帧的地图点投影到当前帧，并且搜索匹配点。用于重定位。

1 int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set<MapPoint*> &sAlreadyFound, const float th, const int ORBdist);

a.计算旋转直方图，用于检测旋转一致性；

b.获取关键帧的地图点，对于每一个地图点：

　　1° 将地图点根据相机模型投影到当前帧的像素坐标系上，且判断是否在CCD平面上；

　　2° 计算预测的尺度层，地图点的深度值必须在图像尺度金字塔内；

　　3° 在预测尺度层上获取特征点；

　　4° 计算该层上的特征点与关键帧中的特征点的描述子距离，取距离最小的。若最小距离小于设定的ORBdist，则为匹配点对；同样需要检测方向（这个还没懂）。

　　5° 用直方图检测描述子方向，和上面一样。

（4）用相似变换投影地图点，并且搜索匹配点。用于闭环检测。

1 int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, std::vector<MapPoint*> &vpMatched, int th);

 a.获取相机标定参数，为之后的投影做准备；

b.分解相似变换矩阵：先计算得到尺度因子s，然后计算世界坐标系到相机坐标系pKF的旋转矩阵和平移向量（都是去掉尺度因子的）。

c.在已经找到的匹配点中，去除没有对应匹配点的地图点；

d.遍历每个地图点，去掉坏的地图点和已经找到的地图点。获取到地图点的3D世界坐标系，并通过上面计算得到的矩阵转换到相机坐标系下，且在Pc下的深度必须为正。再投影到像素坐标系下，坐标值必须在image范围内。计算世界坐标系下，该地图点的深度，该深度必须在该点的尺度方差区域内，且观察视角必须小于60°（通过向量内积来判断观察角度是否小于60°）。根据地图点的深度和关键帧预测一个高斯金字塔层，根据该层计算一个半径，在该半径内搜索最相似的特征点。

3.两个重载的SearchByBoW()函数，搜索关键帧的地图点和某一帧的ORB特征点的匹配点。暴力约束属于相同词典节点的ORB（在一个特定的金字塔层上）。用于重定位和闭环检测。

1 // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
2 // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level)
3 // Used in Relocalisation and Loop Detection
4 int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
5 int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);

第一个函数通过词包对关键帧的特征点进行跟踪，用于重定位。

第二个函数通过词包，对关键帧的特征点进行跟踪，用于闭环检测时对两个关键帧间的特征点匹配。

4.地图初始化时的匹配，仅用于单目情况。

1 int SearchForInitialization(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10); 

5.匹配用于三角化新的地图点，检查对极约束。

1 int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12, std::vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo);

 1.利用基本矩阵F12，在两个关键帧之间未匹配的特征点中产生新的3D点，返回成功匹配的数量。

2.将左图像的每个特征点与右图像同一node节点的所有特征点依次检测，判断是否满足对极约束，满足就是匹配的特征点。

6.搜索在关键帧KF1和关键帧KF2中的地图点匹配，其中KF2是经过KF1相似变换得到，[s12*R12| t12]。在双目和RGB-D情况下，s12=1。

1 int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th);

通过sim3变换，确定pKF1中的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域。在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBow进行特征点匹配时会有漏匹配）。 

7.两个重载的Fuse()函数，用于融合地图点。

（1）将地图点投影至关键帧中，并且搜索复制的地图点。

1 int Fuse(KeyFrame* pKF, const vector<MapPoint *> &vpMapPoints, const float th=3.0); 

（2）将地图点投影至用相似变换得来的关键帧中，并且搜索复制的地图点。

1 int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint);

 

以上几个函数在检测特征点时，都会用到这个函数来计算直方图，还没搞明白。。。

 1 // Rotation Histogram (to check rotation consistency)
 2 vector<int> rotHist[HISTO_LENGTH];
 3 for(int i=0;i<HISTO_LENGTH;i++)
 4      rotHist[i].reserve(500);
 5 const float factor = 1.0f/HISTO_LENGTH;
 6 
 7 if(mbCheckOrientation)
 8 {
 9       float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
10       if(rot<0.0)
11              rot+=360.0f;
12       int bin = round(rot*factor);
13       if(bin==HISTO_LENGTH)
14              bin=0;
15       assert(bin>=0 && bin<HISTO_LENGTH);
16       rotHist[bin].push_back(bestIdx2);
17 }

以及在删除错误匹配对时，会用到下面这部分的代码：

if(mbCheckOrientation)
{
    int ind1=-1;
    int ind2=-1;
    int ind3=-1;

    ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

    for(int i=0; i<HISTO_LENGTH; i++)
    {
        if(i==ind1 || i==ind2 || i==ind3)
             continue;
        for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
        {
             vMatches12[rotHist[i][j]]=-1;
             nmatches--;
         }
     }

}

 角度直方图是用来剔除不满足两帧之间角度旋转的外点的，也就是旋转一致性检测：

1.将关键帧与当前帧匹配点的angle相减，得到rot（0≤rot＜360），放入一个直方图中，对于每一对匹配点的角度差，均可以放入一个bin的范围内（360/HISTO_LENGTH）。

2.统计直方图最高的三个bin保留，其他范围内的匹配点剔除。另外，若最高的比次高的高10倍以上，则只保留最高的bin中的匹配点。

 

protected中：

1.检测极线距离：

1 bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF);

（1）已知两帧图像的对极约束，E或者F，对于第一帧中的关键点x1，其在第二帧中对应的极线l=x1‘F12=[a b c]

（2）点到直线的距离：

设直线 L 的方程为Ax+By+C=0，点 P 的坐标为（Xo，Yo），则点 P 到直线 L 的距离为：

 

 1 bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
 2 {
 3     // Epipolar line in second image l = x1‘F12 = [a b c]
 4     const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
 5     const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
 6     const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
 7 
 8     const float num = a*kp2.pt.x+b*kp2.pt.y+c;
 9 
10     const float den = a*a+b*b;
11 
12     if(den==0)
13         return false;
14 
15     const float dsqr = num*num/den;　　//img2中的关键点到极线的距离的平方
16 
17     return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
18 }

 Q：3.84是什么值？

 

2.计算三个极大值点。

1 void ComputeThreeMaxima(std::vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3);