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