ORB-SLAM ---- Frame::ComputeImageBounds和Frame::AssignFeaturesToGrid()

文章目录

• 一、Frame::ComputeImageBounds()
• • 1. 函数作用及讲解
  • 2. 源码及标注
• 二、Frame::AssignFeaturesToGrid()
• • 1. 函数作用及讲解
  • 2. 源码及标注
  • 3.调用的函数
• 三、总结

一、Frame::ComputeImageBounds()

该函数的作用为计算函数边界，仅在第一帧或者标定参数变化后进行图像边界计算

1. 函数作用及讲解

在去畸变后，图像的边界会发生变化，图像不再是严格的边界，这个时候要找一个最小的矩形，恰好能将原来边界的四个顶点的包含进来

2. 源码及标注

void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
    // 如果畸变参数不为0，用OpenCV函数进行畸变矫正
    if(mDistCoef.at<float>(0)!=0.0)
    {
        // 保存矫正前的图像四个边界点坐标： (0,0) (cols,0) (0,rows) (cols,rows)
        cv::Mat mat(4,2,CV_32F);
        mat.at<float>(0,0)=0.0;             // 左上
        mat.at<float>(0,1)=0.0;
        mat.at<float>(1,0)=imLeft.cols;     // 右上
        mat.at<float>(1,1)=0.0;
        mat.at<float>(2,0)=0.0;             // 左下
        mat.at<float>(2,1)=imLeft.rows;
        mat.at<float>(3,0)=imLeft.cols;     // 右下
        mat.at<float>(3,1)=imLeft.rows;

        // Undistort corners
        // 将mat改为双通道（读取时一次读取两个成员，这里是x和y坐标）
        mat=mat.reshape(2);
        // 调用取畸变点函数
        // 第一二个参数为输入/输出的图像，第三个为相机内参，第四个为畸变系数，第五个为旋转矩阵R（这里表示无旋转），最后一个为新投影矩阵 
        cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
        // 改回单通道
        mat=mat.reshape(1);
        // 去畸变后四个点围成的图形不再是严格的矩形，我们画一个最小能将四个点都装进去的矩形边界
        // 左边界取左上和左下的最小x坐标
        mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));
        // 右边界取右上和右下的最大x坐标
        mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));
        // 上边界取左上和右上的最下y坐标
        mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));
        // 下边界取左下和右下的最大y坐标
        mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));

    }
    else
    {
        // 如果没有畸变就取四个点为边界的四个顶点
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

二、Frame::AssignFeaturesToGrid()

该函数的作用是将特征点分配到相应的网格中

1. 函数作用及讲解

先计算每个网格的预留容量，然后给他分配空间，最后用for循环的方式，将特征点分配到对应的网格内。对于前两步，有人会觉得多此一举，增加代码复杂度，但其实不是这样，这个步骤很重要，ORB-SLAM2之所以从众多视觉SLAM算法中脱颖而出，一个重要原因是他使用的空间小，计算速度快，代码中会出现很多这样的节省空间的操作，不积跬步，无以至千里。

2. 源码及标注

// 将特征点分配到网格中
void Frame::AssignFeaturesToGrid()
{
    // 计算每个网格块的容量，平均一个网格中有多少特征点
    // 这里乘以0.5的原因是，特征点分配不均匀，很多网格分配不到那么多特征点，所以乘以一个系数0.5，用来节省空间
    // 那么大于这个数量的网格会受到影响吗？答案是不会，因为我们用的容器是std::vector动态数组，如果分配空间不足的时候，户自动扩充
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
    
    // 遍历个网格与预留空间
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
        // 给容器预留nReserve这么大的空间
            mGrid[i][j].reserve(nReserve);

    // 遍历每个特征点
    for(int i=0;i<N;i++)
    {
        // 将去畸变后的特征点信息存入kp中
        const cv::KeyPoint &kp = mvKeysUn[i];
        // 存储某个特征点所在网格的网格坐标，nGridPosX范围：[0,FRAME_GRID_COLS], nGridPosY范围：[0,FRAME_GRID_ROWS]
        int nGridPosX, nGridPosY;
        // 将每个特征点分到相应的网格中
        if(PosInGrid(kp,nGridPosX,nGridPosY))
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}

3.调用的函数

Frame::AssignFeaturesToGrid()中调用了 Frame::PosInGrid（）函数，该函数的作用是将特征点分配到网格类，剔除超出边界的特征点

// 确定特征点在网格中的位置，返回值为bool类型
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
    // 计算特征点在哪个网格内
    posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
    posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);

    //Keypoint's coordinates are undistorted, which could cause to go out of the image
    // 超出边界返回false
    if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
        return false;
    // 可以正常分配返回true
    return true;
}

三、总结

在本篇博客写完后双目构造函数就全部结束了，还剩下一些在其他类中直接调用的函数，会在后续讲解，欢迎大家交流指正。