去畸变、计算去畸变图像边界及将特征点分配到对应的网格

这次我们主要讲解单目帧构造函数中的三个具体的函数UndistortKeyPoints，ComputeImageBounds和AssignFeaturesToGrid。

• UndistortKeyPoints主要完成的工作是对特征点去畸变,这个函数只是调用了opencv的去畸变函数，比较简单

  	void Frame::UndistortKeyPoints()
  {
      // Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
  	//变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)
      if(mDistCoef.at<float>(0)==0.0)
      {
          mvKeysUn=mvKeys;
          return;
      }
  
  
      // Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正
      // Fill matrix with points
      // N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中
      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;
      }
  
      // Undistort points
      // 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变
      //为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） 
      mat=mat.reshape(2);
      cv::undistortPoints(	
  		mat,				//输入的特征点坐标
  		mat,				//输出的校正后的特征点坐标覆盖原矩阵
  		mK,					//相机的内参数矩阵
  		mDistCoef,			//相机畸变参数矩阵
  		cv::Mat(),			//一个空矩阵，对应为函数原型中的R
  		mK); 				//新内参数矩阵，对应为函数原型中的P
  	
  	//调整回只有一个通道，回归我们正常的处理方式
      mat=mat.reshape(1);
  
      // Fill undistorted keypoint vector
      // Step 存储校正后的特征点
      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;
      }
  }
  	```
  
  

• ComputeImageBounds这个函数的主要目的是获得去畸变后的图像坐标的范围，为后续的网格划分提供边界值，这个函数只会在第一帧图像进来或者内参发生了改变的时候被调用，这里的边界值选取使用了最大覆盖的准则，也就是左上角取小，右下角取大，从而能够覆盖整个去畸变后的图像

  	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=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;
      }
  }
  

• AssignFeaturesToGrid这个函数的主要目的是根据上一步计算的去畸变后的图像边界和网格尺寸，将去畸变后的特征点分配到对应的网格中去，这么做的目的是为了后面的特征点匹配加速。

  	void Frame::AssignFeaturesToGrid()
  {
      // Step 1  给存储特征点的网格数组 Frame::mGrid 预分配空间
  	// ? 这里0.5 是为什么？节省空间？
      // FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48
      int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
  	//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间
      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中
              mGrid[nGridPosX][nGridPosY].push_back(i);
      }
  }
  

  注意这里的网格中保存的是特征点在特征点向量中的索引

  其中的获取网格坐标的函数如下，注意要剔除掉去畸变后超过图像范围的特征点

  	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);
      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
      // 因为特征点进行了去畸变，而且前面计算是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;
  }