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

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

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

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

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

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

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

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

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

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

// 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 sAlreadyFoundif(!pMP->isBad() && !sAlreadyFound.count(pMP)){//Projectcv::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 levelcv::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 imageif(dist3D<minDistance || dist3D>maxDistance)continue;int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame);// Search in a windowconst 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;
}

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