BEV投影

opencv对于取图上像素的at()操作，编译器自解释：

inline cv::Vec3b &cv::Mat::at<cv::Vec3b>(int row, int col)

 

for (int col{0}; col < pic_cam.cols; col++) {
    for (int row{0}; row < pic_cam.rows; row++) {
      cv::Mat point_mat = (cv::Mat_<double>(3, 1) << col, row, 1.0);    //图上的x y点
      cv::Mat world_point = K_inv * point_mat;                            //归一化坐标系的点
      cv::Mat plane_point = rot_cam_mat * world_point;                    //转到车体系
      if (plane_point.at<double>(0, 2) >= 0) {
        continue;
      }
      double scale = cam_z / plane_point.at<double>(0, 2);                //所有点均为地面点所以高度均为相机高度 ——尺度计算
      plane_point = scale * plane_point;                                    //恢复的地面点
      cv::Mat point_reproject = plane_point - trans_cam_cvm;                //转到车体系

      double point_x = - point_reproject.at<double>(0, 0);                    //小孔成像 成像上xy与现实世界相反
      double point_y = - point_reproject.at<double>(0, 1);

      if (0 < point_x*100 + 500 && point_x*100 + 500 < 1000 &&                //这里的100是随意写的值指的是1M对应100个像素点
          1000 - point_y*100 > 0 && 1000 - point_y*100 < 1000) {                //1000 是最远看10M
        pic_bev.at<cv::Vec3b>(1000 - point_y*100, point_x*100 + 500) =
          pic_cam.at<cv::Vec3b>(row, col);
      }
    }
}

opencv有点坑 每次创建mat都耗时很长 可以作为全局变量重复使用