ORBSLAM3--Frame::GetFeaturesInArea函数

fire mountain

于 2024-04-13 16:02:19 发布

阅读量290

点赞数 2

分类专栏： slam 文章标签： c++ 自动驾驶

本文链接：https://blog.csdn.net/liudacanzn/article/details/137717430

版权

slam 专栏收录该内容

3 篇文章 0 订阅

订阅专栏

文章详细描述了一个名为GetFeaturesInArea的函数，用于在当前帧中通过初始特征点的坐标搜索匹配点。它通过计算搜索范围的边界，遍历符合条件的特征点并进行距离检查。代码中涉及到了栅格化处理、特征点筛选和金字塔层级控制。

摘要由CSDN通过智能技术生成

1.函数的作用GetFeaturesInArea

该函数主要通过初始帧的特征点的坐标在当前帧对应的坐标上查找相对应的特征点。

2.代码解析

  vector<size_t> vIndices;
  vIndices.reserve(N);
  // 搜索半径
  float factorX = r;
  float factorY = r;

定义了存储该出初始帧的特征点在当前帧找到范围内的特征点，还定义搜索半径

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

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

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

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

计算了搜索范围的边界对应的栅格位置

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

                // 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
                for (size_t j = 0, jend = vCell.size(); j < jend; j++)
                {
                    // 根据索引先读取这个特征点
                    const cv::KeyPoint &kpUn = (Nleft == -1) ? mvKeysUn[vCell[j]]
                                               : (!bRight)   ? mvKeys[vCell[j]]
                                                             : mvKeysRight[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) < factorX && fabs(disty) < factorY)
                        vIndices.push_back(vCell[j]);
                }
            }
        }