ORB-SLAM2源码逐行解析系列（五）：ORB-SLAM2中的ORB特征点后处理

1. 去畸变UndistortKeyPoints

/**
 * @brief 用内参对特征点去畸变，结果保存在mvKeysUn中
 * 
 */
void Frame::UndistortKeyPoints()
{
/*
    Step 1 判断是否有必要进行去畸变
	变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)
    第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
    mvKeys是矫正前的特征点    mvKeysUn是矫正后的特征点
*/
    if(mDistCoef.at<float>(0)==0.0)
    {
        mvKeysUn=mvKeys;
        return;
    }
    
    // Step 2 根据opencv的去畸变函数格式，将待去畸变的特征点以cv::Mat的形式进行存储
    cv::Mat mat(N,2,CV_32F);
    for(int i=0; i<N; i++)
    {
		// 然后将这个特征点(需要去畸变的)的横纵坐标分别保存
        mat.at<float>(i,0)=mvKeys[i].pt.x;
        mat.at<float>(i,1)=mvKeys[i].pt.y;
    }
    
    // Step 3 调用opencv的cv::undistortPoints进行去畸变
    mat=mat.reshape(2);
    cv::undistortPoints(	
		mat,				// 输入的特征点坐标
		mat,				// 输出的校正后的特征点坐标，覆盖原矩阵
		mK,					// 相机的内参数矩阵
		mDistCoef,			// 相机畸变参数矩阵
		cv::Mat(),			// 一个空矩阵，对应为函数原型中的R
		mK); 				// 新内参矩阵，对应为函数原型中的P，将原内参矩阵覆盖掉
	
	// 调整回只有一个通道，回归我们正常的处理方式
    mat=mat.reshape(1);
    
    // Step 4 将去畸变后的点存储到mvKeysUn中
    mvKeysUn.resize(N);
    for(int i=0; i<N; i++)
    {
        cv::KeyPoint kp = mvKeys[i];
		// 读取校正后的坐标并覆盖老坐标(只对特征点的坐标进行矫正出处理，其他属性不变)
        kp.pt.x=mat.at<float>(i,0);
        kp.pt.y=mat.at<float>(i,1);
        mvKeysUn[i]=kp;  // 获取校正后的特征点
    }
}

2. 更新图像边界ComputeImageBounds

/**
 * @brief 计算去畸变图像的边界
 * 
 * @param[in] imLeft            需要计算边界的图像
 */
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
		// 和前面校正特征点一样的操作，将这几个(4个)边界点作为输入进行校正
        // 前面是对提取到的所有特征点进行矫正，这里是对4个边界点(mat)进行矫正
        mat=mat.reshape(2);
        cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
        mat=mat.reshape(1);

		// 校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
        mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));  // 左上和左下横坐标最小的
        mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));  // 右上和右下横坐标最大的
        mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));  // 左上和右上纵坐标最小的
        mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));  // 左下和右下纵坐标最大的
    }
    else
    {
        // 如果畸变参数为0，就直接获得图像边界
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

3. 将特征点分配到网格中AssignFeaturesToGrid

/**
 * @brief 将提取的ORB特征点分配到图像网格mGrid中, 后续会用到mGrid这个变量
 */
void Frame::AssignFeaturesToGrid()
{
    // Step 1  给存储特征点的网格数组 Frame::mGrid 预分配空间
	// nReserve表示每个网格中存储的特征点数
    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++)
            mGrid[i][j].reserve(nReserve);

    // Step 2 遍历每个特征点，将每个特征点在mvKeysUn中的索引值放到对应的网格mGrid中
    for(int i=0;i<N;i++)
    {
		// 获取遍历到的特征点(去畸变后的)
        const cv::KeyPoint &kp = mvKeysUn[i];

		// 存储某个特征点所在网格的网格坐标
        // nGridPosX范围：[0,FRAME_GRID_COLS], nGridPosY范围：[0,FRAME_GRID_ROWS]
        int nGridPosX, nGridPosY;
		// 计算某个特征点所在网格的网格坐标
        // 如果找到特征点所在的网格坐标，记录在nGridPosX,nGridPosY里，返回true，没找到返回false
        if(PosInGrid(kp,nGridPosX,nGridPosY))
			// 如果找到特征点所在网格坐标，将这个特征点的索引添加到对应网格中
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}

// PosInGrid的实现
/**
 * @brief 计算某个特征点所在网格的网格坐标，如果找到特征点所在的网格坐标，记录在nGridPosX,nGridPosY里，返回true，没找到返回false
 * 
 * @param[in] kp                    给定的特征点
 * @param[in & out] posX            特征点所在网格坐标的横坐标
 * @param[in & out] posY            特征点所在网格坐标的纵坐标
 * @return true                     如果找到特征点所在的网格坐标，返回true
 * @return false                    没找到返回false
 */
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
/*
	计算特征点x,y坐标落在哪个网格内，网格坐标为posX，posY
    mfGridElementWidthInv=(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
    mfGridElementHeightInv=(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
    (kp.pt.x-mnMinX) / (mnMaxX-mnMinX): 表示当前特征点在图像x方向的比例
    (kp.pt.x-mnMinX) / (mnMaxX-mnMinX) * (FRAME_GRID_COLS): 表示当前特征点的x方向属于图像所划分的哪一个网格
*/
    posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv); 
    // (kp.pt.y-mnMiny) / (mnMaxY-mnMinY): 表示当前特征点在图像y方向的比例
    // (kp.pt.y-mnMiny) / (mnMaxY-mnMinY) * (FRAME_GRID_COLS): 表示当前特征点的y方向属于图像所划分的哪一个网格  
    posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);

    // 因为特征点进行了去畸变，而且前面计算是round取整，所以有可能得到的点落在图像网格坐标外面
    // 如果网格坐标posX，posY超出了[0,FRAME_GRID_COLS] 和[0,FRAME_GRID_ROWS]，表示该特征点没有对应网格坐标，返回false
    if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
        return false;

	// 计算成功返回true
    return true;
}