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++;