目录
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<jend; j++) { // 根据索引先读取这个特征点 const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]]; // 保证给定的搜索金字塔层级范围合法 if(bCheckLevels) { // cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据 // 保证特征点是在金字塔层级minLevel和maxLevel之间,不是的话跳过 if(kpUn.octave<minLevel) 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)<r && fabs(disty)<r) 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红圈范围。
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<jend; j++) { // 根据索引先读取这个特征点 const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]]; // 保证给定的搜索金字塔层级范围合法 if(bCheckLevels) { // cv::KeyPoint::octave中表示的是从金字塔的哪一层提取的数据 // 保证特征点是在金字塔层级minLevel和maxLevel之间,不是的话跳过 if(kpUn.octave<minLevel) 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)<r && fabs(disty)<r) 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容器,返回给上层调用。