ORB_SLAM2特征匹配

SearchByProjection

该方法总体思路是遍历参考帧中所有地图点，分别向当前帧进行投影，在当前帧中找到一个描述子距离最相近的特征点作为其匹配点

使用于运动模型跟踪

函数原型

@ brief 通过投影，对上一帧的特征点进行跟踪
上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints \n
1. 将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
2. 在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
@param CurrentFrame 当前帧
@param LastFrame 上一帧
@param th 阈值
@param bMono 是否为单目
@return 成功匹配的数量
int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);

函数简介

• 函数运用于运动模型的跟踪中，用来完成对上一帧地图点和当前帧特征点的匹配
• 该函数是一个重载函数，它的参数变会有一个是否为单目相机的判断形参，单目和非单目特征点的搜索会有所不同
• 根据上帧的地图点对应特征点所在的金字塔层，来推测当前帧中特征点搜索的半径
• 通过描述子的比较得到最优匹配后，还会对所有完成了匹配的点对创建一个方向变化直方图，用于剔除误匹配概率大的点对

知识难点

由两帧绝对位姿推出两帧相对位姿

1、其中当前帧的位姿$R_{cw}$和$t_{cw}$是已知的分别表示世界坐标系到当前帧的旋转矩阵和平移向量，注意它们的参考坐标都是当前帧相机坐标系，下面我们要计算出以世界坐标系作为参考坐标系的平移向量$t_{wc}(w)$，公式如下：
$$t_{wc}(w)=-{R_{cw}}^{-1}(c)t_{cw}(c)$$
添加负号的原因是$R_{cw}$和$t_{cw}$参考坐标系是相机坐标系，直接相乘得到的是$t_{wc}(c)$，由于平移向量坐标系之间的转换只是符号的差异
2、其中上一帧的位姿$R_{lw}$和$t_{lw}$是已知的分别表示世界坐标系到上一帧的旋转矩阵和平移向量，注意它们的参考坐标都是上一帧相机坐标系，下面我们要计算出以上一帧相机坐标系作为参考坐标系的平移向量$t_{wc}(l)$，公式如下：
$$t_{wc}(l)=R_{lw}{t}_{wc}(w)$$
再者把平移向量相加，注意这里的平移向量是在上一帧l的相机坐标系下的：
$$t_{lc}(l)=t_{wl}(l)+t_{wc}(l)$$

转化示意图

前进与后退对搜索范围的影响

其中尺度想相当于让图像缩小的倍数
以下可以这么理解，例如有一个特征点，在某个尺度n下，以他为原心作一个半径r的圆，最终判定它是一个特征点。当前进时，图像中的像会变大，如果还是以r为半径作圆，圈出来的内容会相比上一时刻变少，判断该点为特征点时也就不能认为是相同的特征点（因为判定信息变化了），那么，则需要在更高的尺度（缩小因子）下重新提取特征点，就相当于对当前帧图像进行缩放到上一帧图像的尺度，使得相同的r下前后时刻框出的数据是一样的，因此m>=n，对应前进的情况，nCurOctave>=nLastOctave。后退的情况可以类推

• 这里的影响是指在非单目情况下，前进和后退都会造成相同的特征在上一帧和当前帧的尺寸发生改变
• 在特征提取中，已经对每一副图像进行了金字塔的计算，每一层的尺度因子（缩小因子）也都计算了出来
• 在基于前进的情况下，当前图像是相比上一图像的同一特征是变大的（物近像大），那么当前帧下检测与上一帧相同的该特征，需要把图像进行缩小，这个就是决定了缩小因子的范围了，也就决定了当前帧下该特征在哪个范围的金字塔层被提取。按照这样的推测可知，当前帧下该特征点被提取金字塔层应该是比上一帧下该特征点被提取的金字塔层要高（相应的尺度也要高），但是不能高出整副图像的金字塔层数。
• 后退情况则相反
• 对于单目情况下就不会进行这个前进和后退的判断，而是直接由上一帧该特征所在的金字塔层决定搜索范围，如下代码片段的else部分
• 整个就是涉及到特征点提取的金字塔、尺度等相关知识
  代码片段

if(bForward) // 前进,则上一帧兴趣点在所在的尺度nLastOctave<=nCurOctave
           vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);//maxOctave=-1
else if(bBackward) // 后退,则上一帧兴趣点在所在的尺度0<=nCurOctave<=nLastOctave
           vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else // 在[nLastOctave-1, nLastOctave+1]中搜索
           vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);

描述子的比较

通过确定搜索范围之后，把这个范围内的特征点全部拿出来，一个一个的和上一帧对应的地图点进行描述子的比较，这实际上就是计算汉明距离的过程。如下代码是作者提供的两个二进制串（256位）得分计算的算法：

// Bit set count operation from比较汉明距离？计算得分？
// 计算两个二进制串相同位不同的数量
// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
    //分别创建了a，b  Mat类的首地址指针
    const int *pa = a.ptr<int32_t>();
    const int *pb = b.ptr<int32_t>();

    int dist=0;
//这里选取的特征点描述子位数是8*32=256位
    //这里就没32位进行一次比较，总共比较8次
    //下面每一次循环其实就是计算一对32位的二进制串的汉明距离方法，是作者参考相应文献得到的
    for(int i=0; i<8; i++, pa++, pb++)
    {
      //异或，相同为0相异为1
        unsigned  int v = *pa ^ *pb;
        v = v - ((v >> 1) & 0x55555555);
        v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
        dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
    }

    return dist;
}

方向一致性检测

• 这里的方向一致性检测是用来剔除误匹配概率大的点对
• 在每一帧的特征点提取中，都会计算每个特征点的的方向，这目的是能在不同帧下相同特征描述子的计算中能实现计算区域不变，做到旋转不变形（旋转了提取的描述子也不变），另一个是用在这里的方向一致性检测，用来剔除误匹配
• 整个的思想是，使用直方图的方法，统计出所有点对得出的前后帧的旋转角度，对于计算出旋转角度最多的点对就很大程度不会是误匹配点，真理掌握在大多数点对中。之后剔除其它点
• 把360度分成30份，那么每一个范围大概就是相差12度左右，但是这里并没有是直接这么分，而是直接把旋转角度除上12得到近似整数后再进行分类（30个类），用二维数组的形式来表示直方图，取出最长的那3行。
• 其中代码注释的补充可以看本次博文中的SearchByBoW对方向一致性检测的总结
  具体代码片段如下

if(mbCheckOrientation)
                    {
		      //理论上，两幅图相同的特征点他们的特征点方向应该是一样的（两个帧都是水平的情况下）
		      //那么前后的相同特征点的方向之差就可以知道图像进行了多少度的旋转
                        float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
                        if(rot<0.0)
                            rot+=360.0f;
			//返回最接近的整数值
			//这里应该是相当于另一种简单思想绘制直方图，如把360度分成12份，对于每一个rot（图像旋转的角度）进行分类
			//只不过这里需要每一次进行rot值的判断（如果330
			//会增加
			//采用下面方法的目的是解决这个时间消耗问题，因为我们只是为了知道某些区间上出现可能情况最多的点对而已，所以
			//直接把rot值除上12，并取这值的近似整数，从这里可以看到，这个直方图并不是12份了，而是30份（360/12=30）,rot
			//值非常接近的的点对情况就会分配到同一个类里面，取3个最大类中的点对作为这次的匹配点对
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)//rot等于360度，相当于没转
                            bin=0;
			//把bin的值固定在0<=bin<30之间，对于角度差太大的点对则会终止整个程序（这会不会太严格了？为什么非要终止整个程序）
                        assert(bin>=0 && bin<HISTO_LENGTH);
			//用所有的特征点对计算以图像旋转多少度作为条件的直方图
			//真理总是掌握在大多数点对手上
			//那么直方图中最高的那一范围就是图像旋转角度的最可能值
			//那么不在这个范围内的点对误匹配的概率就很大，因此就丢弃掉
                        rotHist[bin].push_back(bestIdx2);
                    }
                }
            }
        }
    }

    //Apply rotation consistency
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

	//保留最高的那三条，这3个角度是最接近真实旋转角度的，所以保留这些点对
        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]]=static_cast<MapPoint*>(NULL);
                    nmatches--;
                }
            }
        }
    }

运用于局部地图跟踪

函数原型

/**
 * @brief 通过投影，对Local MapPoint进行跟踪
 *
 * 将Local MapPoint投影到当前帧中, 由此增加当前帧的MapPoints \n
 * 在SearchLocalPoints()中已经将Local MapPoints重投影（isInFrustum()）到当前帧 \n
 * 并标记了这些点是否在当前帧的视野中，即mbTrackInView \n
 * 对这些MapPoints，在其投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
 * @param  F           当前帧
 * @param  vpMapPoints Local MapPoints
 * @param  th          阈值
 * @return             成功匹配的数量
 * @see SearchLocalPoints() isInFrustum()
 */
 //mbTrackInView、mnTrackScaleLevel、mTrackViewCos这些变量是在isInFrustum()函数（Frame成员函数）中进行的赋值
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)

函数简介

该函数还是运用与局部地图的跟踪中，这相比在运动模型跟踪中少个一个传感器形参，不需要进行传感器的判断，没有前进后退造成尺度变化而影响金字塔层搜索范围，具体回看相关介绍。因为局部地图跟踪中，在把局部地图点投影到当前帧下前会对该地图点是否可能被当前帧看到的判断（通过isInFrustum函数），会计算出符合该判断地图点和视角余弦值，和预测出在哪层金字塔中会提取出该对应特征点，会根据这个这两个参数来推测搜索半径和金字塔搜索范围。

知识难点

搜索半径的确定

搜索窗口的大小取决于视角, 若当前视角和平均视角夹角接近0度时, r取一个较小的值，具体代码片段如下

float ORBmatcher::RadiusByViewingCos(const float &viewCos)
{
    if(viewCos>0.998)
        return 2.5;
    else
        return 4.0;
}

viewCos是当前视角和平均视角夹角的余弦值

兴趣特征点搜索

同样还是用GetFeaturesInArea进行兴趣特征点的搜索，只不过相比运动模型的跟踪多了前后退的判断，实现不同情况下的搜索范围进行搜索，这里只有一种情况。

const vector<size_t> vIndices =
                F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel);

• pMP->mTrackProjX,pMP->mTrackProjY是当前pMP地图点投影到当前帧的像素坐标

• nPredictedLevel = pMP->mnTrackScaleLevel，是哪层金字塔中会提取出该对应特征点

• 就是在nPredictedLevel-1和nPredictedLevel这两层进行搜索

与其它重载函数的区别

和运动模型中的重载模型相比

• 不需要进行前进后退的判断，兴趣点搜索只有一种可能
• 没有方向一致性检测
• 搜索半径更复杂，会先计算若当前视角和平均视角夹角余弦，再和对应金字塔尺度相乘，而运行模型只用到金字塔尺度
• 用到了最近邻和第二近邻进行匹配区别度不高的点对进行剔除，而后者没有用到，只用了方向一致性

运用于重定位

总体思想还是相差不大，最终还是通过投影寻找匹配点，在于重定位中，由于经过了多次位姿优化，反复的剔除外点，最终留下的匹配点数量会变少，为了在下次优化中能够有更多的约束，那么会在优化之前进行匹配点数量的添加

函数原型

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

• sAlreadyFound参数是上一次位姿优化或则位姿迭代得到的内点，在各个函数中对这个参数不做人格的改变，所以用了const关键词，只用它进行参考
• ORBdist是汉明距离最大允许阈值
• th是用于计算搜索范围的

函数简介

• 该函数整体上操作和前面的重载函数没有太大差别
• 就多了一个当前地图点是否是存在于sAlreadyFound容器判断，只有不是在该容器的地图点才有添加的必要

知识难点

SearchByBoW

该方法是通过词袋特征向量，也就是正向索引进行加速特征的匹配，词袋一些概念的理解。这里没有投影过程，也不会用金字塔来辅助匹配。
按照BOW向量节点进行匹配有一个明显的优势是在此之前不需要知道任何关于两个匹配对象的几何关系，在第一节中讲到的按照投影的方式进行的匹配，都是给定两匹配对象的初步几何关系，然后根据初步的几何关系确定一个投影区域（投影中心和投影半径），在投影区域内寻找匹配。而相对于投影的匹配方式，按照BOW节点向量进行搜索匹配就不需要预先的集合关系了，按照BOW向量进行的匹配直接根据之前维护的词袋模型，在所有相同bow节点下的特征点中进行搜索匹配，同样更快速。这种方式往往在无法给定两匹配对象之间的初步几何关系时使用。根据匹配对象不同又分为：（1）关键帧和当前帧通过词袋进行快速匹配（1.用在tracking线程根据参考关键帧进行线程追踪过程中2.重定位）（2）关键帧和当前帧通过词袋进行快速匹配（回环检测）

运用于参考关键帧跟踪

函数原型

* @param  pKF               KeyFrame
 * @param  F                 Current Frame
 * @param  vpMapPointMatches F中MapPoints对应的匹配，NULL表示未匹配
 * @return                   成功匹配的数量
 */
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)

函数简介

• 通过bow对pKF和F中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配）
• 对属于同一node的特征点通过描述子距离进行匹配，这里是关键帧pKF的MP和F帧的KP进行描述子距离的计算
• 根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints
• 每个特征点都对应一个MapPoint，因此pKF中每个特征点的MapPoint也就是F中对应点的MapPoint
• 在匹配的初期，pKF的每个地图点都会找一个目前最优的匹配方案，但是会存在很大的误匹配可能，通过距离阈值、比例阈值和角度投票进行剔除误匹配
• vpMapPointMatches存储的是地图点与F帧特征点的对应

知识难点

同样的会进行描述子之间的汉明距离计算还有方向一致检测的方案剔除误匹配，这些和其它的匹配方案中用到的是通用的方法，直接参考对应位置记录

最近邻和第二近邻剔除

使用这个剔除方案目的是计算第一匹配点/第二匹配点的比率（小于1），当比率大时说明第二匹配点和第一匹配点相当，第一匹配点描述子没有特别的“优越性”，在这种情况下不妨舍弃这对匹配，防止发生误匹配

两帧词袋正向索引同结点判断

整个算法实现就如同下面的代码片段，FeatureVector其实就是一个map类，那就可以直接获取它的迭代器进行遍历

DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
    DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
    DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
    DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

    while(KFit != KFend && Fit != Fend)
    {
        if(KFit->first == Fit->first) //步骤1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
        {
	     ........省略
        }
        else if(KFit->first < Fit->first)
        {
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

• 只要pKF或F帧有一个FeatureVector的迭代器遍历到了map容器尾部，就会停止整个循环（同结点查找）
• 第一个if判断如果成立，说明两个迭代器同时访问到了相同的结点，那么就会进入匹配的主程序
• 如果两个迭代器暂时没有访问到相同的结点，就是进行迭代器递增的操作，但是要分成两个情况
• 情况一：如果KFit迭代器访问的结点小于Fit访问的结点，那么KFit就按顺序递增到第一个比Fit访问的结点ID大于或等于的结点上
• 情况二：如果KFit迭代器访问的结点大于于Fit访问的结点，那么Fit就按顺序递增到第一个比KFit访问的结点ID大于或等于的结点上
• 其中lower_bound() 函数的功能就是从数组的begin位置到end-1位置二分查找第一个大于或等于num的数字，找到返回该数字的地址，不存在则返回end。通过返回的地址减去起始地址begin,得到找到数字在数组中的下标。具体参考

方向一致性检测

这里是对SearchByProjection总结里的方向一致性代码注释的一个补充

//同时取出关键帧和这个地图点相匹配的特征点
			//按理来说该关键帧的特征点和该F帧的特征点是出现在相同场景位置的，世界视角中的同一个点
                        const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];

			//这还是在第一个for循环里面
                        if(mbCheckOrientation)
                        {
                            // trick!
                            // angle：每个特征点在提取描述子时的旋转主方向角度，如果图像旋转了，这个角度将发生改变
                            // 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值，就该角度
                            // 出现最多的点就认为是准确的角度变化值，不在这个范围内的点认为是误匹配
                            // 这里的变化值是指图像旋转的角度值，如果前后帧图像没发生旋转，理论上所有的rot是等于零的
                            float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值
                            if(rot<0.0)
                                rot+=360.0f;
                            int bin = round(rot*factor);// 将rot分配到bin组
                            if(bin==HISTO_LENGTH)
                                bin=0;
			    //不满足则终止程序，如果不满足就会超出了这个rotHist的空间范围，会出现内存泄露，可能结果很严重所以终止
                            assert(bin>=0 && bin<HISTO_LENGTH);
                            rotHist[bin].push_back(bestIdxF);
                        }
                        nmatches++;
                    }
                }

            }

            KFit++;
            Fit++;
        }
        else if(KFit->first < Fit->first)
        {
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

    // 根据方向剔除误匹配的点
    if(mbCheckOrientation)
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        // 计算rotHist中最大的三个的index，列树最大的3行
        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            // 如果特征点的旋转角度变化量属于这三个组，则保留
            if(i==ind1 || i==ind2 || i==ind3)
                continue;

            // 将除了ind1 ind2 ind3以外的匹配点去掉
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {
                vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }