目录

1.函数作用

2.函数解析 

2.0 代码

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

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

1.函数作用

        找到在 以x,y为中心，半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点。传入的r为windowSize = 100。

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const

2.函数解析 

2.0 代码

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const
{
	// 存储搜索结果的vector
    vector<size_t> 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<size_t> vCell = mGrid[ix][iy];
			// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
            if(vCell.empty())
                continue;
 
            // 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
            for(size_t j=0, jend=vCell.size(); j
            {
				// 根据索引先读取这个特征点 
                const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
				// 保证给定的搜索金字塔层级范围合法
                if(bCheckLevels)
                {
					// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据
					// 保证特征点是在金字塔层级minLevel和maxLevel之间，不是的话跳过
                    if(kpUn.octave
                        continue;
                    if(maxLevel>=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)fabs(disty)
                    vIndices.push_back(vCell[j]);
            }
        }
    }
    return vIndices;
}

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

    vector<size_t> 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);

        这里我们初始化了一个vector容器vIndices存储了与帧1特征点所在栅格对应的帧2栅格所在栅格内所有的特征点的索引。

 

         mfGridElementWidthInv在下方我的博客中计算，mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX)。这个变量的意义是栅格的列数/图像的宽度，举个例子，比如要把10米分成5个栅格，那么5/10=0.5，用这个数去乘以当前图像的位置就可以得到在哪个栅格中，比如有个物体在7m处，7*0.5=3.5，那么这个物体就在第三个栅格中。

         这里nMinCellX 定义为x-mnMinX-r，这个距离即mnMinX到圆左面切线的距离，这个距离乘以mfGridElementWidthInv再向下取整就得到了所在行栅格的坐标，其余算法同理，算完之后就得到了特征点的网格范围。即帧2红圈范围。

ORB-SLAM2 ---- Frame::AssignFeaturesToGrid函数_Courage2022的博客-CSDN博客https://blog.csdn.net/qq_41694024/article/details/126316590

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

for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            // 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
            const vector<size_t> vCell = mGrid[ix][iy];
			// 如果这个网格中没有特征点，那么跳过这个网格继续下一个
            if(vCell.empty())
                continue;
 
            // 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
            for(size_t j=0, jend=vCell.size(); j
            {
				// 根据索引先读取这个特征点 
                const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
				// 保证给定的搜索金字塔层级范围合法
                if(bCheckLevels)
                {
					// cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据
					// 保证特征点是在金字塔层级minLevel和maxLevel之间，不是的话跳过
                    if(kpUn.octave
                        continue;
                    if(maxLevel>=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)fabs(disty)
                    vIndices.push_back(vCell[j]);
            }
        }
    }

        解释一下里面的几个变量：

        mGrid：这是在提取图像特征点的时候在去畸变得到去畸变的特征点容器mvKeysUn后，将一帧分割FRAME_GRID_COLS*FRAME_GRID_ROWS即64*48个栅格，并用mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]容器存放mvKeysUn的索引值。

        在这个循环中，我们从上到下从左到右遍历每个栅格：

        用vCell 变量存储在该栅格中的所有特征点的索引，如果这个栅格为空，则遍历下一个栅格。如果这个栅格中有特征点存在，则遍历每一个特征点，检查其是否满足层数要求。再检查这些特征点是否满足在规定的范围windowsize（其实是个圆，但栅格是矩形，即排除外点）内，如果据满足，将特征点的索引加入vIndices容器，返回给上层调用。