ORB-SLAM（八）ORBmatcher 特征匹配

该类负责特征点与特征点之间，地图点与特征点之间通过投影关系、词袋模型或者Sim3位姿匹配。用来辅助完成单目初始化，三角化恢复新的地图点，tracking，relocalization以及loop closing，因此比较重要。

该类提供的API是：

1. 几个重载的SearchByProjection函数（第一个形参代表需要在其中寻找匹配点的当前图像帧/query；第二个形参则包含待匹配特征/train），用于

　　a. 跟踪局部地图(在局部地图中寻找与当前帧特征点匹配的)。因为在TrackReferenceKeyFrame和TrackWithMotionModel中，仅仅是两帧之间跟踪，会跟丢地图点，这里通过跟踪局部地图，在当前帧中恢复出一些当前帧的地图点。　　其中的阈值th一般根据单目还是双目，或者最近有没有进行过重定位来确定，代表在投影点的这个平面阈值范围内寻找匹配特征点。匹配点不仅需要满足对极几何，初始位姿的约束；还需要满足描述子之间距离较小。

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th);

　　b. 匹配上一帧的地图点，即前后两帧匹配，用于TrackWithMotionModel。

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);

　　c. 在当前帧中匹配所有关键帧中的地图点，用于Relocalization。

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

　　d. 在当前关键帧中匹配所有关键帧中的地图点，需要计算sim3，用于Loop Closing。

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

2. 两个重载的SearchByBow函数（注意这里形参表示的匹配的主被动关系和SearchByProjection是反的），用于

　　a. 在当前帧中匹配关键帧中的地图点，用于TrackReferenceKeyFrame和Relocalization。

int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches);

　　b. 在当前关键帧中匹配所有关键帧中的地图点，用于Loop Closing。

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12);

3. 用于单目初始化的SearchForInitialization，以及利用三角化，在两个关键帧之间恢复出一些地图点SearchForTriangulation。

int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize);
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo);

4. 两个重载的Fuse函数，用于地图点的融合：

　　地图点能匹配上当前关键帧的地图点，也就是地图点重合了，选择观测数多的地图点替换；地图点能匹配上当前帧的特征点，但是该特征点还没有生成地图点，则生成新的地图点）。

　　重载的函数是为了减小尺度漂移的影响，需要知道当前关键帧的sim3位姿。

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

5. 计算描述子之间的hanmming距离

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

选取其中一个用于Relocalization的投影匹配着重理解。疑问是，何时用投影匹配，何时用DBow2进行匹配？在Relocalization和LoopClosing中进行匹配的是在很多帧关键帧集合中匹配，属于Place Recognition，因此需要用DBow，而投影匹配适用于两帧之间，或者投影范围内（局部地图，前一个关键帧对应地图点）的MapPoints与当前帧之间。

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

用关键帧pKF的地图点投影匹配当前帧的特征点：

// For Relocalization
//
// 1. 获取pKF对应的地图点vpMPs，遍历
//     (1). 若该点为NULL、isBad或者在SearchByBow中已经匹配上（Relocalization中首先会通过SearchByBow匹配一次），抛弃；
// 2. 通过当前帧的位姿，将世界坐标系下的地图点坐标转换为当前帧坐标系（相机坐标系）下的坐标
//     (2). 投影点(u,v)不在畸变矫正过的图像范围内，地图点的距离dist3D不在地图点的可观测距离内（根据地图点对应的金字塔层数，
//           也就是提取特征的neighbourhood尺寸），抛弃
// 3. 通过地图点的距离dist3D，预测特征对应金字塔层nPredictedLevel，并获取搜索window大小（th*scale），在以上约束的范围内，
//    搜索得到候选匹配点集合向量vIndices2
//     const vector<size_t> vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nPredictedLevel-1, nPredictedLevel+1);
// 4. 计算地图点的描述子和候选匹配点描述子距离，获得最近距离的最佳匹配，但是也要满足距离<ORBdist。
// 5. 最后，还需要通过直方图验证描述子的方向是否匹配
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)
{
    int nmatches = 0;

    const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
    const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
    const cv::Mat Ow = -Rcw.t()*tcw;

    // Rotation Histogram (to check rotation consistency)
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = 1.0f/HISTO_LENGTH;

    const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();

    for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
    {
        MapPoint* pMP = vpMPs[i];

        if(pMP)
        {
            // before this, Relocalization has already execute SearchByBoW, those matched was inserted into sAlreadyFound
            if(!pMP->isBad() && !sAlreadyFound.count(pMP))
            {
                //Project
                cv::Mat x3Dw = pMP->GetWorldPos();
                cv::Mat x3Dc = Rcw*x3Dw+tcw;

                const float xc = x3Dc.at<float>(0);
                const float yc = x3Dc.at<float>(1);
                const float invzc = 1.0/x3Dc.at<float>(2);

                const float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
                const float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
                // u,v是关键帧中地图点在当前帧上的投影点
                if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
                    continue;
                if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
                    continue;

                // Compute predicted scale level
                cv::Mat PO = x3Dw-Ow;
                float dist3D = cv::norm(PO);

                const float maxDistance = pMP->GetMaxDistanceInvariance();
                const float minDistance = pMP->GetMinDistanceInvariance();

                // Depth must be inside the scale pyramid of the image
                if(dist3D<minDistance || dist3D>maxDistance)
                    continue;

                int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame);

                // Search in a window
                const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel];

                const vector<size_t> vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nPredictedLevel-1, nPredictedLevel+1);

                if(vIndices2.empty())
                    continue;

                const cv::Mat dMP = pMP->GetDescriptor();

                int bestDist = 256;
                int bestIdx2 = -1;

                for(vector<size_t>::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
                {
                    const size_t i2 = *vit;
                    if(CurrentFrame.mvpMapPoints[i2])
                        continue;

                    const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);

                    const int dist = DescriptorDistance(dMP,d);

                    if(dist<bestDist)
                    {
                        bestDist=dist;
                        bestIdx2=i2;
                    }
                }

                if(bestDist<=ORBdist)
                {
                    CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
                    nmatches++;

                    if(mbCheckOrientation)
                    {
                        float rot = pKF->mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);
                        rotHist[bin].push_back(bestIdx2);
                    }
                }

            }
        }
    }

    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)
            {
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    CurrentFrame.mvpMapPoints[rotHist[i][j]]=NULL;
                    nmatches--;
                }
            }
        }
    }

    return nmatches;
}

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

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

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

最后该函数会

1. 为当前帧生成和关键帧匹配上的地图点

2. 统计通过投影匹配上的点

CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
nmatches++;