orb-slam2 从单目开始的简单学习（4）：match

前言

本章不是作为主线，而是作为主线的辅助理解

1. Search

常见函数	用途
GetFeaturesInArea	返回的以x,y为中心，半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点

可以打开文档之后ctrl+f寻找自己不了解的函数
【参考文档】orb-slam2 从单目开始的简单学习（6）Frame

1.1 SearchByBoW

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

1.1.1数据类型 和 基本函数

1.1.1.1.数据类型

数据名	类型
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> >，概括起来就是词袋特征向量

1.1.1.2.基本函数

KFit = vFeatVecKF.lower_bound(Fit->first);

vFeatVecKF底层是一个map，因此lower_bound也是map的一个方法

terator lower_bound(const key_type& _Keyval)：返回一个迭代器，指向键值 >= _Keyval 的第一个元素；

1.1.2 对于词袋的理解

进一步了解BOW建议是移步到computeBOW

对于每一个节点应该是两个维度上的观察
竖直维度上：节点之间相互连接
水平维度上：该节点上对应的索引

画了个二维理解图希望能帮助理解

1.1.3 完整代码

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

1.1.4代码分块详解

这个博主注释上算是比较好理解的。理解这一堆代码的核心还是在树和map的映射上。

1）构建旋转直方图

将两个要匹配的特征点的方向做差得到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

2）if-elif-else

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中反复提及的唯一性 ，有纪念意义

• 唯一性：通过寻找当前帧与关键帧最相似的点
  寻找方法：KP 送进BOW结点中，在节点中进一步寻找最小距离
• 有纪念意义：记录显著方向的

另一个重载

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

其实也差不多，节省篇幅就不上全部代码了
只不过就是变成了两个都是关键帧

1.2 SearchByProjection

1.2.1解析

利用重投影在其投影点附近根据描述子距离选取匹配，由此增加当前帧的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);

应该是第三种，因此着重讲解第三种，这篇中有对四种进行讲解

1.2.1.1完整代码

1.2.2代码分块解析

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

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

th：搜索范围阈值
bMono：是否为单目相机

1) 位姿关系

//当前相机坐标系到世界坐标系的平移向量
    const cv::Mat twc = -Rcw.t()*tcw;

《slam14讲》p44
基于特殊欧式群的性质：求解该矩阵的逆表示一个反向的变换
特殊欧式群：

其中矩阵T称为变化矩阵

该矩阵的逆：

const cv::Mat tlc = Rlw*twc+tlw;

//截图过来

2）针对双目/RGB-D 相机向前/后

// 仅针对双目或RGB-D相机
    const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;
    const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;

3）小孔成像模型

代码的整体逻辑是

• 以下是三维世界中的MapPoint点到像素坐标的计算过程
  1.世界坐标–>相机坐标；
  2.相机坐标–>相机归一化平面坐标；
  3.相机归一化平面坐标–>像素坐标；

	  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

4） GetFeaturesInArea

【参考文档】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);

5）搜索半径限制

双目和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;
}

1.3. SearchForInitialization

2.Fuse

将地图点与帧中图像的特征点进行匹配，实现地图点融合

// 将地图点投影到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

基于公式

3. SearchForTriangulation

3.1 极点的理解

极点可以理解为 ：第一帧图像相机光心在第二帧坐标系中的位置

3.2 完整代码和注释

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

• vMatchedPairs：得到的是两帧图片对应的关键点在对应帧中序号
• 序号对应关系：关键点，地图点，未矫正的点