ORB-SLAM2 ---- Frame::GetFeaturesInArea函数

目录

1.函数用处

2.步骤 

3.code 

4.函数解释 

4.1 函数思想

4.2 代码解释 

---

1.函数用处

        找到在 以为中心，半径为的圆形内且金字塔层级在的特征点。

2.步骤 

Step 1 计算半径为r圆左右上下边界所在的网格列和行的id

Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里

3.code 

vector Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const
{
	// 存储搜索结果的vector
    vector vIndices;
    vIndices.reserve(N);

    // Step 1 计算半径为r圆左右上下边界所在的网格列和行的id
    // 查找半径为r的圆左侧边界所在网格列坐标。这个地方有点绕，慢慢理解下：
    // (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素（肯定大于1）
    // mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒数，表示每个像素可以均分几个网格列（肯定小于1）
	// (x-mnMinX-r)，可以看做是从图像的左边界mnMinX到半径r的圆的左边界区域占的像素列数
	// 两者相乘，就是求出那个半径为r的圆的左侧边界在哪个网格列中
    // 保证nMinCellX 结果大于等于0
    const int nMinCellX = max(0,(int)floor( (x-mnMinX-r)*mfGridElementWidthInv));


	// 如果最终求得的圆的左边界所在的网格列超过了设定了上限，那么就说明计算出错，找不到符合要求的特征点，返回空vector
    if(nMinCellX>=FRAME_GRID_COLS)
        return vIndices;

	// 计算圆所在的右边界网格列索引
    const int nMaxCellX = min((int)FRAME_GRID_COLS-1, (int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
	// 如果计算出的圆右边界所在的网格不合法，说明该特征点不好，直接返回空vector
    if(nMaxCellX<0)
        return vIndices;

	//后面的操作也都是类似的，计算出这个圆上下边界所在的网格行的id
    const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
    if(nMinCellY>=FRAME_GRID_ROWS)
        return vIndices;

    const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
    if(nMaxCellY<0)
        return vIndices;

    // 检查需要搜索的图像金字塔层数范围是否符合要求
    //? 疑似bug。(minLevel>0) 后面条件 (maxLevel>=0)肯定成立
    //? 改为 const bool bCheckLevels = (minLevel>=0) || (maxLevel>=0);
    const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);

    // Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
    for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            // 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
            const vector vCell = mGrid[ix][iy];
			// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
            if(vCell.empty())
                continue;

            // 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
            for(size_t j=0, jend=vCell.size(); j=0)		//? 为何特意又强调？感觉多此一举
                        if(kpUn.octave>maxLevel)
                            continue;
                }               

                // 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
                const float distx = kpUn.pt.x-x;
                const float disty = kpUn.pt.y-y;

				// 如果x方向和y方向的距离都在指定的半径之内，存储其index为候选特征点
                if(fabs(distx)

4.函数解释 

4.1 函数思想

        对于ORB-SLAM2这个算法来讲，它是认为相邻两帧间的运动微乎其微，于是，我们在匹配特征点时就有以下思想：

        由于两帧之间的运动不是很大，那么第一帧中的某一特征点的位置与第二帧中该特征点的位置应该差不多，于是我们就有了这个函数，我们将第一帧中的某一特征点在相机坐标系的位置记录，然后画一个区域在第二帧的该区域寻找所有特征点，找到与该特征点最相似的一个特征点（通过描述子距离比较）作为匹配的特征点。

        于是，我们看看是怎么做的吧！

        先看一下变量含义和一些量的含义：

        ①mnMinX：该帧的左边界点

        ②mnMaxX：该帧的左边界点

        ③FRAME_GRID_COLS：一个帧的列数量（如上图：一个帧分为了很多个格子）

        ④(mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每个网格可以平均分得几个像素

        ⑤mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是④的倒数，表示每个像素可以均分几个网格列

        ⑥x-mnMinX-r：特征点的x-帧的左边界-半径=圆的最左面的点，即mMinCellX的坐标

        ⑦(x-mnMinX-r)*mfGridElementWidthInv：这个值向下取整就表示圆的最左面的点所在哪列（在哪个列格子里）

        懂了这些我们就能读懂这块的代码了。

4.2 代码解释 

        我们现根据传入的参数确定搜索的框框（确定一个方形区域，这个区域包含了当前的特征点，我们将这个框框投影到下一帧在下一帧的这个框框中取出所有的特征点挨个匹配）

        这个框框是由nMinCellX、nMaxCellX、nMinCellY、nMaxCellY围成的矩形区域。        

        遍历圆形区域内的每个网格，寻找满足条件的候选特征点：

        获取一个网格内的所有特征点在 Frame::mvKeysUn 中的索引（去畸变后的特征点）（在第二帧中的特征点的索引）存到vCell向量中，我们读取每个特征点放在临时变量kpUn中，判断这个特征点的合法性（金字塔层级范围是否合法，通过传入的参数minLevel，maxLevel确定），我们再判断这个特征点到“圆心的距离r（函数形参）”（我们找到的特征点是遍历每个矩形的，这是为了清楚矩形边缘的特征点）是否合理，若合理，我们将此特征点的索引放入Indice向量中。

        当我们遍历所有的栅格的时候，我们就得到了与第一帧中的某一特征点待匹配的所有特征点的索引了。