本章不是作为主线，而是作为主线的辅助理解
| 常见函数 | 用途 |
|---|---|
| GetFeaturesInArea | 返回的以x,y为中心，半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点 |
可以打开文档之后ctrl+f寻找自己不了解的函数
【参考文档】orb-slam2 从单目开始的简单学习（6）Frame
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
| 数据名 | 类型 |
|---|---|
| FeatureVector | map |
// Vector of nodes with indexes of local features具有局部特征索引的节点向量
class FeatureVector:
public std::map<NodeId, std::vector<unsigned int> >
NodeId：节点向量（可以理解为树干）
vector :局部特征索引（可以理解为该树干对应的叶子）
vFeatVecKF，F.mFeatVec本质上都是上面这个map<NodeId, std::vector<unsigned int> >，概括起来就是词袋特征向量
KFit = vFeatVecKF.lower_bound(Fit->first);
vFeatVecKF底层是一个map，因此lower_bound也是map的一个方法
terator lower_bound(const key_type& _Keyval)：返回一个迭代器，指向键值 >= _Keyval 的第一个元素；
进一步了解BOW建议是移步到computeBOW
对于每一个节点应该是两个维度上的观察
竖直维度上：节点之间相互连接
水平维度上：该节点上对应的索引
画了个二维理解图希望能帮助理解
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();
//返回mvpMapPoints：与KeyPoint相关联的MapPoints
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));
// F.N：Number of KeyPoints.
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
int nmatches=0;
//----------------为了检查KeyPoints方向性做准备-----------------
vector<int> rotHist[HISTO_LENGTH];//const int ORBmatcher::HISTO_LENGTH = 30;
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = 1.0f/HISTO_LENGTH;
//----------------为了检查KeyPoints方向性做准备-----------------
//--------------为了遍历KeyPoints做准备----------------
// 对于同一结点上的orb进行匹配
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();//map类型
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
//vFeatVecKF，F.mFeatVec都属于FeatureVector
//FeatureVector:map<NodeId, std::vector<unsigned int> >
//--------------为了遍历KeyPoints做准备-------------
//-------------开始遍历-------------------
//FeatureVector:map<node_id,std::vector<feature_id>>
while(KFit != KFend && Fit != Fend)
{
//具有一定相似性的会集中在同一个结点下
//为了确保其唯一性，需要找到vector中最好的
if(KFit->first == Fit->first)
{
const vector<unsigned int> vIndicesKF = KFit->second;
const vector<unsigned int> vIndicesF = Fit->second;
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
const unsigned int realIdxKF = vIndicesKF[iKF];
MapPoint* pMP = vpMapPointsKF[realIdxKF];
if(!pMP)
continue;
if(pMP->isBad())
continue;
//-------确认KeyPoint的唯一性（最小/次小 bit距离）------------------------
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
int bestDist1=256;
int bestIdxF =-1 ;
int bestDist2=256;
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
const unsigned int realIdxF = vIndicesF[iF];
if(vpMapPointMatches[realIdxF])//已经进入过下面vpMapPointMatches[bestIdxF]=pMP;
continue;
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
//orb描述子，每一行与一个关键点相关联
const int dist = DescriptorDistance(dKF,dF);//按位计算距离
if(dist<bestDist1)//找最小距离
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
}
else if(dist<bestDist2)//次小距离 bestDist1<dist<bestDist2
{
bestDist2=dist;
}
}//最小和次小距离：描述当前结点下的帧中与关键帧中kp【最匹配的】的
//--------------为检查方向做准备----------------
if(bestDist1<=TH_LOW)//const int ORBmatcher::TH_LOW = 50;
{
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
vpMapPointMatches[bestIdxF]=pMP;
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
if(mbCheckOrientation)
{
float rot = kp.angle-F.mvKeys[bestIdxF].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(bestIdxF);
}
nmatches++;
}
}
//--------------为检查方向做准备----------------
//-------确认KeyPoint的唯一性（最小/次小距离）------------------------
}
KFit++;
Fit++;
}
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
//-------确认KeyPoint的 有纪念意义（方向）------------------------
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++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
//-------确认KeyPoint的有 纪念意义（方向显著）------------------------
return nmatches;
}
这个博主注释上算是比较好理解的。理解这一堆代码的核心还是在树和map的映射上。
将两个要匹配的特征点的方向做差得到rot,将rot转换到0-360度,设定一个直方图,直方图的HISTO_LENGTH即条形的个数设置为30,0-360分布在30个bins中,即每个bin代表12度.然后根据角度差将n对匹配特征点分布在直方图中,在直方图中找出三个匹配特征点最多的直方图bin,最后将不属于这三个bin的匹配关系删除
———————————————— 版权声明：本文为CSDN博主「Bobsweetie」的原创文章，遵循CC 4.0
BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/Bobsweetie/article/details/107367464
vector<int> rotHist[HISTO_LENGTH];//const int ORBmatcher::HISTO_LENGTH = 30;
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = 1.0f/HISTO_LENGTH;
可以理解成分成30个基本块，每个块的高度最多500
while(KFit != KFend && Fit != Fend)
{
if(KFit->first == Fit->first)
{
KFit++;
Fit++;
}
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
KFit->first == Fit->first寻找相同结点（基于前面对于词袋的理解）
KFit = vFeatVecKF.lower_bound(Fit->first)可以理解为将相同结点对齐
还是和我在tracking中反复提及的唯一性 ，有纪念意义
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
其实也差不多，节省篇幅就不上全部代码了
只不过就是变成了两个都是关键帧
利用重投影在其投影点附近根据描述子距离选取匹配，由此增加当前帧的MapPoints 。在TrackWithMotionModel()中使用
重投影：利用将相机坐标系下（三维）的Local MapPoints投影到图像坐标系（二维）。
共有四种：
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int 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)
根据TrackWithMotionModel()中代码
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
应该是第三种，因此着重讲解第三种，这篇中有对四种进行讲解
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。
th：搜索范围阈值
bMono：是否为单目相机
//当前相机坐标系到世界坐标系的平移向量
const cv::Mat twc = -Rcw.t()*tcw;
《slam14讲》p44
基于特殊欧式群的性质：求解该矩阵的逆表示一个反向的变换
特殊欧式群：
其中矩阵T称为变化矩阵
该矩阵的逆：
const cv::Mat tlc = Rlw*twc+tlw;
//截图过来
// 仅针对双目或RGB-D相机
const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;
代码的整体逻辑是
const float invzc = 1.0/x3Dc.at<float>(2)//归一化平面
float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
看到uv应该很熟悉，即就是经典的世界坐标系转相机坐标系
vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
【参考文档】orb-slam2 从单目开始的简单学习（6）Frame详见GetFeaturesInArea有完整介绍
根据相机的前后前进方向来判断搜索尺度范围。
当相机前进时，原来的特征点需要在更高的尺度下才能找到正确的匹配点
当相机后退时，原来的特征点需要在更低的尺度下才能找到正确的匹配点
金字塔结构图：
层级越高，所看见的内容在实际场景中越大
if(bForward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else//非双目情况
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
双目和rgbd的情况，需要保证右图的点也在搜索半径以内
if(CurrentFrame.mvuRight[i2]>0)
{ //（u，v）
const float ur = u - CurrentFrame.mbf*invzc;// mbf:Stereo baseline multiplied by fx.
//invzc:归一化参数
const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
if(er>radius)
continue;
}
将地图点与帧中图像的特征点进行匹配，实现地图点融合
// 将地图点投影到KeyFrame并搜索重复的地图点
int Fuse(KeyFrame* pKF, const vector<MapPoint *> &vpMapPoints, const float th=3.0);
// 使用给定的Sim3将MapPoints投影到KeyFrame中，并搜索重复的MapPoints。
int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint);
有两个重载
const float ur = u-bf*invz
基于公式
极点可以理解为 ：第一帧图像相机光心在第二帧坐标系中的位置
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;
//-------step 1. 计算在第二帧中极点---------------------------------Compute epipole（baseline和平面的连线） in second image
//极点可以理解为第一帧图像相机光心在第二帧坐标系中的位置
cv::Mat Cw = pKF1->GetCameraCenter();//pKF1光心在世界坐标系中的坐标
cv::Mat R2w = pKF2->GetRotation();//世界坐标系到相机坐标系
cv::Mat t2w = pKF2->GetTranslation();
cv::Mat C2 = R2w*Cw+t2w;//将pKF1光心从世界坐标系转移到pKF2相机坐标系
const float invz = 1.0f/C2.at<float>(2);
const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;//归一化u
const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;//归一化v
// 查找未跟踪关键点之间的匹配项Find matches between not tracked keypoints
// 通过ORB词汇表，加快匹配速度Matching speed-up by ORB Vocabulary
// 仅比较共享同一节点的ORBCompare only ORB that share the same node
int nmatches=0; //先进行预设值
vector<bool> vbMatched2(pKF2->N,false);
vector<int> vMatches12(pKF1->N,-1);//记录第二张图中第一张图的对应匹配点序号
vector<int> rotHist[HISTO_LENGTH];//每一个rotHist[i]都是一个vector<int>类型
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);//bin的最大高度是500
const float factor = 1.0f/HISTO_LENGTH;
//-------step2 寻找图2中图1的匹配点------------
//FeatureVector本质上是map<NodeId, std::vector<unsigned int> >
//NodeId：代表节点 vector<unsigned int>：映射在同一节点的关键点的索引
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();
//索引对应关系：关键点，地图点，描述子
while(f1it!=f1end && f2it!=f2end)
{//-------step2.1对应相同节点通过寻找最小距离来进行匹配---------
if(f1it->first == f2it->first)
{//-------step2.1.1遍历第一张图中该节点所有对应的关键点-----
for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
{
const size_t idx1 = f1it->second[i1];//取索引
MapPoint* pMP1 = pKF1->GetMapPoint(idx1);//取索引对应的地图点
// 跳过已经存在地图点的 If there is already a MapPoint skip
if(pMP1)
continue;
const bool bStereo1 = pKF1->mvuRight[idx1]>=0;//判断是否为双目
if(bOnlyStereo)
if(!bStereo1)
continue;
const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];//取索引对应的未矫正的关键点
const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
int bestDist = TH_LOW;
int bestIdx2 = -1;
//-------step2.1.2遍历第二张图中该节点所有对应的关键点以寻找最小距离------
for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
{
size_t idx2 = f2it->second[i2];
MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
// If we have already matched or there is a MapPoint skip
if(vbMatched2[idx2] || pMP2)
continue;
const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
if(bOnlyStereo)
if(!bStereo2)
continue;
const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
const int dist = DescriptorDistance(d1,d2);
if(dist>TH_LOW || dist>bestDist)
continue;
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
if(!bStereo1 && !bStereo2)
{// 极点到kp2的像素距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
const float distex = ex-kp2.pt.x;
const float distey = ey-kp2.pt.y;
if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
continue;
}
if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
{
bestIdx2 = idx2;
bestDist = dist;
}
}
if(bestIdx2>=0)
{
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
vMatches12[idx1]=bestIdx2;
nmatches++;//观测点
if(mbCheckOrientation)
{
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);
}
}
//-------step3 只记录前三方向变化点--------------
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++)
{
vMatches12[rotHist[i][j]]=-1;
nmatches--;
}
}
}
vMatchedPairs.clear();
vMatchedPairs.reserve(nmatches);
//-------step4 将匹配特征点对在帧中序号放入vMatchedPairs--------------
for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
{
if(vMatches12[i]<0)//被初始化为-1，会被过滤
continue;
vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
//i代表第一帧中的关键点，vMatches12[i]代表第二帧中对应的匹配点
}
return nmatches;
}