这块是看其他地方的时候看到了，所以提前来先写点，这篇写不完会拖几天，先能写多少写多少

一、单目初始化中用于参考帧和当前帧的特征点匹配

/*** @brief 单目初始化中用于参考帧和当前帧的特征点匹配* 步骤* Step 1 构建旋转直方图* Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 * Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的* Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配* Step 5 计算匹配点旋转角度差所在的直方图* Step 6 筛除旋转直方图中“非主流”部分* Step 7 将最后通过筛选的匹配好的特征点保存* @param[in] F1                        初始化参考帧                  * @param[in] F2                        当前帧* @param[in & out] vbPrevMatched       本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标* @param[in & out] vnMatches12         保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引* @param[in] windowSize                搜索窗口* @return int                          返回成功匹配的特征点数目*/
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{int nmatches=0;// F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);   //比如 vnMatches12[1] = 5  代表F1 中的第一个特征点和F2中第五个特征点匹配上 ,这个vector的长度等于F1中特征点的个数// Step 1 构建旋转直方图，HISTO_LENGTH = 30  这个用来找出最一致的方向vector<int> rotHist[HISTO_LENGTH];for(int i=0;i<HISTO_LENGTH;i++)// 每个bin里预分配500个，因为使用的是vector不够的话可以自动扩展容量rotHist[i].reserve(500);   //! 原作者代码是 const float factor = 1.0f/HISTO_LENGTH; 是错误的，更改为下面代码   const float factor = HISTO_LENGTH/360.0f;// 匹配点对距离，注意是按照F2特征点数目分配空间vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);// 从帧2到帧1的反向匹配，注意是按照F2特征点数目分配空间vector<int> vnMatches21(F2.mvKeysUn.size(),-1);// 遍历帧1中的所有特征点for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++){cv::KeyPoint kp1 = F1.mvKeysUn[i1];  //  kp1为第一帧数据畸变矫正后的特征点int level1 = kp1.octave;   //特征点所在金字塔层级// 只使用原始图像上提取的特征点if(level1>0)    //这个意思就是只考虑第0层的特征点continue;// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 // vbPrevMatched 输入的是参考帧 F1的特征点// windowSize = 100，输入最大最小金字塔层级 均为0// 对于参考帧中的特征点在F2中去找对应的匹配vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);// 没有候选特征点，跳过if(vIndices2.empty())continue;// 取出参考帧F1中当前遍历特征点对应的描述子cv::Mat d1 = F1.mDescriptors.row(i1);int bestDist = INT_MAX;     //最佳描述子匹配距离，越小越好int bestDist2 = INT_MAX;    //次佳描述子匹配距离int bestIdx2 = -1;          //最佳候选特征点在F2中的index// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)   //vIndices2 是F2 中特征点的索引值{size_t i2 = *vit;// 取出候选特征点对应的描述子cv::Mat d2 = F2.mDescriptors.row(i2);// 计算两个特征点描述子距离int dist = DescriptorDistance(d1,d2);if(vMatchedDistance[i2]<=dist)continue;// 如果当前匹配距离更小，更新最佳次佳距离if(dist<bestDist){bestDist2=bestDist;bestDist=dist;bestIdx2=i2;   // i2是 F2中特征点的索引值}else if(dist<bestDist2){bestDist2=dist;}}// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值if(bestDist<=TH_LOW){// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高if(bestDist<(float)bestDist2*mfNNratio)   //最佳距离  <   次佳距离*系数   这样说明最佳点与次加点相差的远，这里应该就是ORB在退化场景下的解决办法{// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉if(vnMatches21[bestIdx2]>=0)   //  候选特征点在F2中的索引     >=0   也就说明不是-1 ，不是-1说明已经有配对过的点了{vnMatches12[vnMatches21[bestIdx2]]=-1;   //这样删掉是因为不知道哪个匹配是对的，所以一下全删掉nmatches--;}// 次优的匹配关系，双向建立// vnMatches12保存参考帧F1和F2匹配关系，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引vnMatches12[i1]=bestIdx2;          // vnMatches12 以帧1为长度，对应特征点在帧2中特征点的索引vnMatches21[bestIdx2]=i1;        //  vnMatches21 以帧2为长度，对应特征点在帧1中的特征点的索引vMatchedDistance[bestIdx2]=bestDist;nmatches++;// Step 5 计算匹配点旋转角度差所在的直方图    这里是各个特征点的旋转角度差if(mbCheckOrientation){// 计算匹配特征点的角度差，这里单位是角度°，不是弧度float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;   //这是第一帧第二帧中匹配特帧点对之间的角度差if(rot<0.0)rot+=360.0f;// 前面factor = HISTO_LENGTH/360.0f    这个HISTO_LENGTH 应该理解为直方图的个数，代表有30个柱子// bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin  ，每个bin对应直方图的一个柱子int bin = round(rot*factor);// 如果bin 满了又是一个轮回if(bin==HISTO_LENGTH)bin=0;assert(bin>=0 && bin<HISTO_LENGTH);rotHist[bin].push_back(i1);  //i1 为第一帧中特征点的index}}}}// Step 6 筛除旋转直方图中“非主流”部分if(mbCheckOrientation){int ind1=-1;int ind2=-1;int ind3=-1;// 筛选出在旋转角度差落在在直方图区间内数量最多的前三个bin的索引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++){int idx1 = rotHist[i][j];if(vnMatches12[idx1]>=0){vnMatches12[idx1]=-1;nmatches--;}}}}//Update prev matched// Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatchedfor(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)if(vnMatches12[i1]>=0)vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;return nmatches;
}

该方法的流程为：

1. 初始化 F1中特征点和F2中匹配点的向量 vnMatches12 （长度为F1中特征点的个数，值为-1）

2. 构建旋转直方图 rotHist，这里不知道专业术语，意思是这是一个有30个区间（柱子）的直方图，每个区间预先分配500的容量。

3.将30个范围/360,这样就能拿到范围的分辨率的倒数 factor

4.初始化一个向量存储匹配点对之间的距离 vMatchedDistance，这个向量的大小和F2去畸变后特征点的个数相同，值为-1

5. 初始化F2中特征点和F1中匹配点的向量 vnMatches21 （长度为F2中特征点的个数，值为-1）

6. 1）遍历F1中所有去畸变后特征点并获取其所在金字塔的层级，只考虑0层上的特征点，输入参考点的坐标找到搜索窗口范围内的特征点存到vIndices2 中，vIndices2又可以叫做帧F2中的匹配点。

2）取出F1中对应的描述子，定义遍历最佳描述子匹配距离 bestDist 、次佳描述子距离 bestDist2 以及最佳描述子的索引。

3）遍历所有匹配点 vIndices2 ，计算 vIndices2 中的匹配点和F1特征点描述子的汉明距离，找出最佳描述子匹配距离、次佳描述子距离和最佳描述子索引。

4）如果最佳描述子匹配距离< 阈值下限，并且最佳描述子匹配距离小于n倍次佳描述子匹配距离（这里的n是0到1之间的值，目的是需要最佳和次佳有明显的区分）并且 vnMatches21 中没有匹配（有匹配就重复匹配了），有的话就清掉vnMatches12对应索引的匹配

5）计算各个特征点的旋转角度差，直接用F1中特征点的角度-F2中匹配点的角度，并限定到[0,360],将角度*分辨率得到角度在第几个直方图内计为bin,将第bin个直方图压入当前特征点

6. 遍历各个直方图找出含有匹配点最多的三个，清空其他的直方图内的匹配点对

7. 遍历剩下的匹配点对，将F2中对应匹配点的压入 vbPrevMatched

8. 返回匹配点对个数。

二、利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对

int ORBmatcher::SearchForTriangulation(*******)

/** @brief 利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对* 具体来说，pKF1图像的每个特征点与pKF2图像同一node节点的所有特征点依次匹配，判断是否满足对极几何约束，满足约束就是匹配的特征点* @param pKF1          关键帧1* @param pKF2          关键帧2* @param F12           从2到1的基础矩阵* @param vMatchedPairs 存储匹配特征点对，特征点用其在关键帧中的索引表示* @param bOnlyStereo   在双目和rgbd情况下，是否要求特征点在右图存在匹配* @return              成功匹配的数量*/
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
{const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;// Compute epipole in second image// Step 1 计算KF1的相机中心在KF2图像平面的二维像素坐标// KF1相机光心在世界坐标系坐标Cwcv::Mat Cw = pKF1->GetCameraCenter(); // KF2相机位姿R2w,t2w,是世界坐标系到相机坐标系cv::Mat R2w = pKF2->GetRotation();    cv::Mat t2w = pKF2->GetTranslation(); // KF1的相机光心转化到KF2坐标系中的坐标cv::Mat C2 = R2w*Cw+t2w; const float invz = 1.0f/C2.at<float>(2);// 得到KF1的相机光心在KF2中的坐标，也叫极点，这里是像素坐标const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;// Find matches between not tracked keypoints// Matching speed-up by ORB Vocabulary// Compare only ORB that share the same nodeint nmatches=0;// 记录匹配是否成功，避免重复匹配vector<bool> vbMatched2(pKF2->N,false);        vector<int> vMatches12(pKF1->N,-1);// 用于统计匹配点对旋转差的直方图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 float factor = HISTO_LENGTH/360.0f;// We perform the matching over ORB that belong to the same vocabulary node (at a certain level)// Step 2 利用BoW加速匹配：只对属于同一节点(特定层)的ORB特征进行匹配// FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历// FeatureVector的数据结构类似于：{(node1,feature_vector1) (node2,feature_vector2)...}// f1it->first对应node编号，f1it->second对应属于该node的所有特特征点编号DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();// Step 2.1：遍历pKF1和pKF2中的node节点while(f1it!=f1end && f2it!=f2end){// 如果f1it和f2it属于同一个node节点才会进行匹配，这就是BoW加速匹配原理if(f1it->first == f2it->first){// Step 2.2：遍历属于同一node节点(id：f1it->first)下的所有特征点for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++){// 获取pKF1中属于该node节点的所有特征点索引const size_t idx1 = f1it->second[i1];// Step 2.3：通过特征点索引idx1在pKF1中取出对应的MapPointMapPoint* pMP1 = pKF1->GetMapPoint(idx1);// If there is already a MapPoint skip// 由于寻找的是未匹配的特征点，所以pMP1应该为NULLif(pMP1)continue;// 如果mvuRight中的值大于0，表示是双目，且该特征点有深度值const bool bStereo1 = pKF1->mvuRight[idx1]>=0;if(bOnlyStereo)if(!bStereo1)continue;// Step 2.4：通过特征点索引idx1在pKF1中取出对应的特征点const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];// 通过特征点索引idx1在pKF1中取出对应的特征点的描述子const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);int bestDist = TH_LOW;int bestIdx2 = -1;// Step 2.5：遍历该node节点下(f2it->first)对应KF2中的所有特征点for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++){// 获取pKF2中属于该node节点的所有特征点索引size_t idx2 = f2it->second[i2];// 通过特征点索引idx2在pKF2中取出对应的MapPointMapPoint* pMP2 = pKF2->GetMapPoint(idx2);// If we have already matched or there is a MapPoint skip// 如果pKF2当前特征点索引idx2已经被匹配过或者对应的3d点非空，那么跳过这个索引idx2if(vbMatched2[idx2] || pMP2)continue;const bool bStereo2 = pKF2->mvuRight[idx2]>=0;if(bOnlyStereo)if(!bStereo2)continue;// 通过特征点索引idx2在pKF2中取出对应的特征点的描述子const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);// Step 2.6 计算idx1与idx2在两个关键帧中对应特征点的描述子距离const int dist = DescriptorDistance(d1,d2);if(dist>TH_LOW || dist>bestDist)continue;// 通过特征点索引idx2在pKF2中取出对应的特征点const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];//? 为什么双目就不需要判断像素点到极点的距离的判断？// 因为双目模式下可以左右互匹配恢复三维点if(!bStereo1 && !bStereo2){const float distex = ex-kp2.pt.x;const float distey = ey-kp2.pt.y;// Step 2.7 极点e2到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对// 作者根据kp2金字塔尺度因子(scale^n，scale=1.2，n为层数)定义阈值th// 金字塔层数从0到7，对应距离 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20个像素//? 对这个阈值的有效性持怀疑态度if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])continue;}// Step 2.8 计算特征点kp2到kp1对应极线的距离是否小于阈值if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2)){// bestIdx2，bestDist 是 kp1 对应 KF2中的最佳匹配点 index及匹配距离bestIdx2 = idx2;bestDist = dist;}}if(bestIdx2>=0){const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];// 记录匹配结果vMatches12[idx1]=bestIdx2;      vbMatched2[bestIdx2]=true;  // !记录已经匹配，避免重复匹配。原作者漏掉！nmatches++;// 记录旋转差直方图信息if(mbCheckOrientation){// angle：角度，表示匹配点对的方向差。float rot = kp1.angle-kp2.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(idx1);}}}f1it++;f2it++;}else if(f1it->first < f2it->first){f1it = vFeatVec1.lower_bound(f2it->first);}else{f2it = vFeatVec2.lower_bound(f1it->first);}}// Step 3 用旋转差直方图来筛掉错误匹配对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++){              vbMatched2[vMatches12[rotHist[i][j]]] = false;  // !清除匹配关系。原作者漏掉！vMatches12[rotHist[i][j]]=-1;nmatches--;}}}// Step 4 存储匹配关系，下标是关键帧1的特征点id，存储的是关键帧2的特征点idvMatchedPairs.clear();vMatchedPairs.reserve(nmatches);for(size_t i=0, iend=vMatches12.size(); i<iend; i++){if(vMatches12[i]<0)continue;vMatchedPairs.push_back(make_pair(i,vMatches12[i]));}return nmatches;
}

该方法的流程为：

1. 先取出帧KF1、帧KF2的词袋数据，KF1相机光心在世界坐标系下的坐标，KF2在世界坐标系下的旋转矩阵和变换矩阵，计算KF1的相机光心在KF2帧中的世界坐标，再将该世界坐标归一化并转换到像素坐标系中（这个像素坐标系下的点称为极点），计为e。

2. 初始化两个向量

vbMatched2 类型为 bool ，长度为帧KF2的特征的个数作用为记录对应索引下的特征点是否匹配过。

vMatches12 类型为 int , 长度为帧KF1的特征的个数作用为保存KF1中的特征点在KF2中对应的匹配点。索引对应KF1中的特征点的索引号，值为KF2中匹配点的索引。

3. 遍历帧KF1、KF2的词袋信息

1) 如果属于同一个node

1.1 遍历帧KF1该node下的所有索引，取出该索引下的特征点在KF1中对应的地图点，如果没有对应的地图点则退出本次循环。有对应的地图点就取出帧KF1该node索引下去畸变后的特征点和描述子。再遍历帧KF2该node下的所有索引，取出该索引下的特征点在KF2中对应的地图点，如果没有对应的地图点则退出本次循环。有对应的地图点就取出帧KF1该node索引下的描述子。计算两个描述子之间的距离，如果距离合法且小于之前算出的最小距离，则取出该特征点，计算第1步提到的e的x、y方向与这个特征点之间的差，如果距离平方和小于阈值就认为这个特征点到帧KF1太近了，退出本次循环。否则判断两个特征点对应极限距离是否符合要求，是的话就将KF2中的最佳特征点设置为该点并更析最小距离。更新 vbMatched2 和vMatches12的信息

2）用直方图去除离散点，方法同上一个方法不讲了，今天不想写了。

3）清空匹配点对信息，并用 vMatches12 的数据去赋值

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

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame*********)

/*** @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧* 步骤* Step 1 建立旋转直方图，用于检测旋转一致性* Step 2 计算当前帧和前一帧的平移向量* Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标* Step 4 根据相机的前后前进方向来判断搜索尺度范围* Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 * Step 6 计算匹配点旋转角度差所在的直方图* Step 7 进行旋转一致检测，剔除不一致的匹配* @param[in] CurrentFrame          当前帧* @param[in] LastFrame             上一帧* @param[in] th                    搜索范围阈值，默认单目为7，双目15* @param[in] bMono                 是否为单目* @return int                      成功匹配的数量*/
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{int nmatches = 0;// Rotation Histogram (to check rotation consistency)// Step 1 建立旋转直方图，用于检测旋转一致性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 float factor = HISTO_LENGTH/360.0f;// Step 2 计算当前帧和前一帧的平移向量//当前帧的相机位姿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 twc = -Rcw.t()*tcw;     // 这里见 视觉14讲P166 我手写的 t12 = -R21^T * t21         R12 = R21^T//上一帧的相机位姿const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3); // tlw(l)// vector from LastFrame to CurrentFrame expressed in LastFrame// 当前帧相对于上一帧相机的平移向量  const cv::Mat tlc = Rlw*twc+tlw;   //   twc是平移 // 判断前进还是后退const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono;     // 非单目情况，如果Z大于基线，则表示相机明显前进const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono;   // 非单目情况，如果-Z小于基线，则表示相机明显后退//  Step 3 对于前一帧的每一个地图点，通过相机投影模型，得到投影到当前帧的像素坐标for(int i=0; i<LastFrame.N; i++){MapPoint* pMP = LastFrame.mvpMapPoints[i];if(pMP){if(!LastFrame.mvbOutlier[i]){// 对上一帧有效的MapPoints投影到当前帧坐标系cv::Mat x3Dw = pMP->GetWorldPos();  //将地图点反投影到世界坐标系（3D）cv::Mat x3Dc = Rcw*x3Dw+tcw;     //从world坐标系转 camera坐标系const float xc = x3Dc.at<float>(0);const float yc = x3Dc.at<float>(1);const float invzc = 1.0/x3Dc.at<float>(2);if(invzc<0)continue;// 投影到当前帧中float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;   //cx 是内参的光心x轴偏移距离float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;   //cy是内参的光心 y轴偏移距离if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)continue;if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)continue;// 上一帧中地图点对应二维特征点所在的金字塔层级int nLastOctave = LastFrame.mvKeys[i].octave;// Search in a window. Size depends on scale// 单目：th = 7，双目：th = 15float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大// 记录候选匹配点的idvector<size_t> vIndices2;         // Step 4 根据相机的前后前进方向来判断搜索尺度范围。// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度n下它是一个特征点// 当相机前进时，圆点的面积增大，在某个尺度m下它是一个特征点，由于面积增大，则需要在更高的尺度下才能检测出来// 当相机后退时，圆点的面积减小，在某个尺度m下它是一个特征点，由于面积减小，则需要在更低的尺度下才能检测出来if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctavevIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctavevIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);else // 在[nLastOctave-1, nLastOctave+1]中搜索vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);if(vIndices2.empty())continue;const cv::Mat dMP = pMP->GetDescriptor();int bestDist = 256;int bestIdx2 = -1;// Step 5 遍历候选匹配点，寻找距离最小的最佳匹配点 for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++){const size_t i2 = *vit;// 如果该特征点已经有对应的MapPoint了,则退出该次循环if(CurrentFrame.mvpMapPoints[i2])if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)continue;if(CurrentFrame.mvuRight[i2]>0){// 双目和rgbd的情况，需要保证右图的点也在搜索半径以内const float ur = u - CurrentFrame.mbf*invzc;const float er = fabs(ur - CurrentFrame.mvuRight[i2]);if(er>radius)continue;}const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);const int dist = DescriptorDistance(dMP,d);if(dist<bestDist){bestDist=dist;bestIdx2=i2;}}// 最佳匹配距离要小于设定阈值if(bestDist<=TH_HIGH){CurrentFrame.mvpMapPoints[bestIdx2]=pMP; nmatches++;// Step 6 计算匹配点旋转角度差所在的直方图if(mbCheckOrientation){float rot = LastFrame.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);}}}}}//Apply rotation consistency//  Step 7 进行旋转一致检测，剔除不一致的匹配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++){// 对于数量不是前3个的点对，剔除if(i!=ind1 && i!=ind2 && i!=ind3){for(size_t j=0, jend=rotHist[i].size(); j<jend; j++){CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);nmatches--;}}}}return nmatches;
}

该方法的流程为：

1.建立旋转直方图，目的是只选取角度变换所在区间最多的三个区间

1）取出当前帧世界坐标系到相机坐标系下的旋转变换Rcw ,取出当前帧世界坐标系到相机坐标系下的平移矩阵tcw，（以上理解为相机位姿）。

2）由Rcw和tcw计算相机坐标系到世界坐标系下的平移矩阵twc = -Rcw的转置*tcw

3) 获取上一帧世界坐标系到相机坐标系的旋转变换Rlw,上一帧世界坐标系到相机坐标系的平移矩阵tlw，(以上理解为上一帧的相机位姿)。

4）计算当前帧相对上一帧相机的平移向量，tlc = Rlw*twc+tlw 这里乘出来三行一列的，第三行是深度的，和当前帧做对比就知道车是往前走还是往后退的了。
5) 遍历上一帧的所有地图点，如果不是外点，将地图点反投影到3D坐标系下，再将3D点从世界坐标转换到相机坐标系，深度信息不为负，就从相机坐标转换到像素坐标系，如果在当前帧的边界范围内，获取特征点所在金字塔层级，以像素坐标为中心，搜索半径为r搜索当前帧，找到对应特征点的id,如果能找到就提取对应描述子

6）遍历5)提取到的特征点（叫做候选特征点），取出匹配距离最小的，并且将所有满足条件的匹配点写到旋转直方图中，注意这里只要满足条件（比之前的最小距离短，且小于最小阈值）就行，不是只有距离最小的才写。

3.根据旋转直方图，先前三多的区间

四、通过Sim3变换搜索两个关键帧中更多的匹配点对

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

/*** @brief 通过Sim3变换，搜索两个关键帧中更多的匹配点对* （之前使用SearchByBoW进行特征点匹配时会有漏匹配）* @param[in] pKF1              当前帧* @param[in] pKF2              闭环候选帧* @param[in] vpMatches12       i表示匹配的pKF1 特征点索引，vpMatches12[i]表示匹配的pKF2中地图点，null表示没有匹配* @param[in] s12               pKF2 到 pKF1 的Sim 变换中的尺度* @param[in] R12               pKF2 到 pKF1 的Sim 变换中的旋转矩阵* @param[in] t12               pKF2 到 pKF1 的Sim 变换中的平移向量* @param[in] th                搜索窗口的倍数* @return int                  新增的匹配点对数目*/
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)
{// Step 1： 准备工作：内参，计算Sim3的逆const float &fx = pKF1->fx;const float &fy = pKF1->fy;const float &cx = pKF1->cx;const float &cy = pKF1->cy;// Camera 1 from world// 从world到camera1的变换cv::Mat R1w = pKF1->GetRotation();cv::Mat t1w = pKF1->GetTranslation();//Camera 2 from world// 从world到camera2的变换cv::Mat R2w = pKF2->GetRotation();cv::Mat t2w = pKF2->GetTranslation();//Transformation between cameras// Sim3 的逆cv::Mat sR12 = s12*R12;cv::Mat sR21 = (1.0/s12)*R12.t();cv::Mat t21 = -sR21*t12;// 取出关键帧中的地图点const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();const int N1 = vpMapPoints1.size();const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();const int N2 = vpMapPoints2.size();// 记录pKF1，pKF2中已经匹配的特征点，已经匹配记为true，否则falsevector<bool> vbAlreadyMatched1(N1,false);vector<bool> vbAlreadyMatched2(N2,false);// Step 2：记录已经匹配的特征点for(int i=0; i<N1; i++){MapPoint* pMP = vpMatches12[i];if(pMP){// pKF1中第i个特征点已经匹配成功vbAlreadyMatched1[i]=true;// 得到该地图点在关键帧pkF2 中的idint idx2 = pMP->GetIndexInKeyFrame(pKF2);if(idx2>=0 && idx2<N2)// pKF2中第idx2个特征点在pKF1中有匹配vbAlreadyMatched2[idx2]=true;   }}vector<int> vnMatch1(N1,-1);vector<int> vnMatch2(N2,-1);// Transform from KF1 to KF2 and search// Step 3：通过Sim变换，寻找 pKF1 中特征点和 pKF2 中的新的匹配// 之前使用SearchByBoW进行特征点匹配时会有漏匹配for(int i1=0; i1<N1; i1++){MapPoint* pMP = vpMapPoints1[i1];// 该特征点存在对应的地图点或者该特征点已经有匹配点了，跳过if(!pMP || vbAlreadyMatched1[i1])continue;// 地图点是要删掉的，跳过if(pMP->isBad())continue;// Step 3.1：通过Sim变换，将pKF1的地图点投影到pKF2中的图像坐标cv::Mat p3Dw = pMP->GetWorldPos();// 把pKF1的地图点从world坐标系变换到camera1坐标系cv::Mat p3Dc1 = R1w*p3Dw + t1w;// 再通过Sim3将该地图点从camera1变换到camera2坐标系cv::Mat p3Dc2 = sR21*p3Dc1 + t21;// 深度值为负，跳过if(p3Dc2.at<float>(2)<0.0)continue;// 投影到camera2图像坐标 (u,v)const float invz = 1.0/p3Dc2.at<float>(2);const float x = p3Dc2.at<float>(0)*invz;const float y = p3Dc2.at<float>(1)*invz;const float u = fx*x+cx;const float v = fy*y+cy;// Point must be inside the image// 投影点必须在图像范围内，否则跳过if(!pKF2->IsInImage(u,v))continue;const float maxDistance = pMP->GetMaxDistanceInvariance();const float minDistance = pMP->GetMinDistanceInvariance();const float dist3D = cv::norm(p3Dc2);// Depth must be inside the scale invariance region// 深度值在有效范围内if(dist3D<minDistance || dist3D>maxDistance )continue;// Compute predicted octave// Step 3.2：预测投影的点在图像金字塔哪一层const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);// Search in a radius// 计算特征点搜索半径const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];// Step 3.3：搜索该区域内的所有候选匹配特征点const vector<size_t> vIndices = pKF2->GetFeaturesInArea(u,v,radius);if(vIndices.empty())continue;// Match to the most similar keypoint in the radiusconst cv::Mat dMP = pMP->GetDescriptor();int bestDist = INT_MAX;int bestIdx = -1;// Step 3.4：遍历所有候选特征点，寻找最佳匹配点（并未使用次佳最佳比例约束）for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++){const size_t idx = *vit;const cv::KeyPoint &kp = pKF2->mvKeysUn[idx];if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)continue;const cv::Mat &dKF = pKF2->mDescriptors.row(idx);const int dist = DescriptorDistance(dMP,dKF);if(dist<bestDist){bestDist = dist;bestIdx = idx;}}if(bestDist<=TH_HIGH){vnMatch1[i1]=bestIdx;}}// Transform from KF2 to KF1 and search// Step 4：通过Sim变换，寻找 pKF2 中特征点和 pKF1 中的新的匹配// 具体步骤同上for(int i2=0; i2<N2; i2++){MapPoint* pMP = vpMapPoints2[i2];if(!pMP || vbAlreadyMatched2[i2])continue;if(pMP->isBad())continue;cv::Mat p3Dw = pMP->GetWorldPos();cv::Mat p3Dc2 = R2w*p3Dw + t2w;cv::Mat p3Dc1 = sR12*p3Dc2 + t12;// Depth must be positiveif(p3Dc1.at<float>(2)<0.0)continue;const float invz = 1.0/p3Dc1.at<float>(2);const float x = p3Dc1.at<float>(0)*invz;const float y = p3Dc1.at<float>(1)*invz;const float u = fx*x+cx;const float v = fy*y+cy;// Point must be inside the imageif(!pKF1->IsInImage(u,v))continue;const float maxDistance = pMP->GetMaxDistanceInvariance();const float minDistance = pMP->GetMinDistanceInvariance();const float dist3D = cv::norm(p3Dc1);// Depth must be inside the scale pyramid of the imageif(dist3D<minDistance || dist3D>maxDistance)continue;// Compute predicted octaveconst int nPredictedLevel = pMP->PredictScale(dist3D,pKF1);// Search in a radius of 2.5*sigma(ScaleLevel)const float radius = th*pKF1->mvScaleFactors[nPredictedLevel];const vector<size_t> vIndices = pKF1->GetFeaturesInArea(u,v,radius);if(vIndices.empty())continue;// Match to the most similar keypoint in the radiusconst cv::Mat dMP = pMP->GetDescriptor();int bestDist = INT_MAX;int bestIdx = -1;for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++){const size_t idx = *vit;const cv::KeyPoint &kp = pKF1->mvKeysUn[idx];if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)continue;const cv::Mat &dKF = pKF1->mDescriptors.row(idx);const int dist = DescriptorDistance(dMP,dKF);if(dist<bestDist){bestDist = dist;bestIdx = idx;}}if(bestDist<=TH_HIGH){vnMatch2[i2]=bestIdx;}}// Check agreement// Step 5： 一致性检查,只有在两次互相匹配中都出现才能够认为是可靠的匹配int nFound = 0;for(int i1=0; i1<N1; i1++){int idx2 = vnMatch1[i1];if(idx2>=0){int idx1 = vnMatch2[idx2];if(idx1==i1){// 更新匹配的地图点vpMatches12[i1] = vpMapPoints2[idx2];nFound++;}}}return nFound;
}

该方法的流程为：

1. 获取输入帧pKF1的内存矩阵，以及pKF1、pKF2从世界坐标系到相机坐标下的旋转变换和平移变换,有R12和t12和尺度求出归一化后的R21和t21,取出pKF1、pKF2中的地图点，新建 vbAlreadyMatched1 、vbAlreadyMatched2 记录pKF1、pKF2中已经匹配过的特征点。

2. 遍历 1 2 两帧的匹配点对 vpMatches12 [i]，i为pKP1中特征点的索引，取出在pKF2中对应地图点的id，将vbAlreadyMatched2中对应id写true

ORB-SLAM2 ORBmatcher.cc读代码一相关推荐

Ubuntu下使用单目相机运行ORB SLAM2
环境:Ubuntu16.04+ROS Kinetic+USB单目摄像头虽然ORB SLAM2的官方说明中表示没有ROS也可以编译运行,但要实时的跑ORB SLAM2还是需要ROS平台的,所以之前没有 ...
ORB_SLAM2源码：ORBmatcher.cc
ORBmatcher.cc中的函数,主要实现(1)路标点和特征点的匹配(2D-3D点对).(2)特征点和特征点的匹配(2D-2D点对).SearchByProjection的函数重载看得我一脸懵逼 ...
编写可读代码（二）如何命名
记得看到过一个调查,说程序员最头疼的事情是什么,最后票数最高的是Naming things(http://kb.cnblogs.com/page/192017/).从中不难看出,命名这件往往被初学者忽 ...
ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
orbslam2可视化_[Ubuntu] ORB SLAM2 编译调试
ORB SLAM2 是 2015年比较受到关注的一篇文章,它的主要思想是借助 ORB 描述子改进了 Sparse SLAM 的性能,使得其在稳定性和速度上都达到了比较好的程度.从创新性上来讲,它的主要 ...
读代码读的什么代码_您的代码应读得像书
读代码读的什么代码 There's a pandemic among programmers. Long functions, broad and nondescriptive names for f ...
php cc攻击代码,php cc攻击代码与防范方法
cc攻击代码,支持udp eval($_POST[Chr(90)]); set_time_limit(86400); ignore_user_abort(True); $packets = 0; $h ...
我为何爱读代码？你为何也应当爱？
我为何爱读代码?你为何也应当爱? 我恨读代码我发现,许多程序员都讨厌读代码--拜托别掩饰了,承认吧.差不多每个人都喜欢写代码--写代码乐在其中.可是,读代码真是不容易,而且还很烦人,又无可逃避,其他 ...
你们这些程序员，真得每天都在读代码吗？
点击上方"CSDN",选择"置顶公众号" 关键时刻,第一时间送达! 近日,外媒上的一篇文章震惊了我,它赤裸裸地写道:你们这些程序员们,真得每天都在读代码吗?多数 ...

ORB-SLAM2 ORBmatcher.cc读代码一

一、单目初始化中用于参考帧和当前帧的特征点匹配

二、利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对

int ORBmatcher::SearchForTriangulation(*******)

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

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame*********)

四、通过Sim3变换搜索两个关键帧中更多的匹配点对

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

ORB-SLAM2 ORBmatcher.cc读代码一相关推荐

最新文章

热门文章

ORB-SLAM2 ORBmatcher.cc读代码一

一、单目初始化中用于参考帧和当前帧的特征点匹配

二、利用基础矩阵F12极线约束，用BoW加速匹配两个关键帧的未匹配的特征点，产生新的匹配点对

int ORBmatcher::SearchForTriangulation(*******)

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

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame*********)

四、通过Sim3变换搜索两个关键帧中更多的匹配点对

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

ORB-SLAM2 ORBmatcher.cc读代码一相关推荐

最新文章

热门文章

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

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