OpenCV——点云投影到图像中

读取一张照片和一张 pcd， 根据标定的内参和外参，将点云投影到图像中，用于判断雷达相机外参标定是否准确。

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>

#include <iostream>

int main(int argc, char** argv)
{
  // read a image and a pcd
  cv::Mat image_origin = cv::imread("/media/data/temp/image/0.jpeg");
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_origin(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_withoutNAN(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::io::loadPCDFile<pcl::PointXYZI> ("/media/liuzhiyang/data/temp/pcd/0.pcd", *cloud_origin);
  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*cloud_origin, *cloud_withoutNAN, indices);

  std::vector<cv::Point3f> pts_3d;
  for (size_t i = 0; i < cloud_withoutNAN->size(); ++i)
  {
    pcl::PointXYZI point_3d = cloud_withoutNAN->points[i];
    if (point_3d.x > 2 && point_3d.x < 3 && point_3d.y > -10 && point_3d.y < 10)
    {
      pts_3d.emplace_back(cv::Point3f(point_3d.x, point_3d.y, point_3d.z));
    }
  }

  // using iterator

  // read calibration parameter
  double fx = 1.0757955405501191e+03, fy = 1.0762345733674481e+03;
  double cx = 9.6249394948422218e+02, cy = 6.1957628038839391e+02;
  double k1 = -1.1995613777994101e-01, k2 = 8.6245969435724004e-02, k3 = -2.6778267188218002e-02;
  double p1 = 1.0621717082800000e-03, p2 = 5.4033385896265832e-04;
  cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0);
  cv::Mat distortion_coeff = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3); 
  cv::Mat r_vec = (cv::Mat_<double>(3, 1) << 1.29949179254383, -1.113823535227475, 1.108412921650477);
  cv::Mat t_vec = (cv::Mat_<double>(3, 1) << -0.370740907093656, -0.2397403632299851, -0.0407927826288379);
  
  // project 3d-points into image view
  std::vector<cv::Point2f> pts_2d;
  cv::projectPoints(pts_3d, r_vec, t_vec, camera_matrix, distortion_coeff, pts_2d);
  cv::Mat image_project = image_origin.clone();
  int image_rows = image_origin.rows;
  int image_cols = image_origin.cols;

  for (size_t i = 0; i < pts_2d.size(); ++i)
  {
    cv::Point2f point_2d = pts_2d[i];
    if (point_2d.x < 0 || point_2d.x > image_cols || point_2d.y < 0 || point_2d.y > image_rows)
    {
      continue;
    }
    else
    {
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[0] = 0;
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[1] = 0;
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[2] = 255;
    }
    
    if (point_2d.x > 0 && point_2d.x < image_cols && point_2d.y > 0 && point_2d.y < image_rows)
    {
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[0] = 0;
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[1] = 0;
      image_project.at<cv::Vec3b>(point_2d.y, point_2d.x)[2] = 255;
    } 
    else
    {
      continue;
    }  
  }

  cv::imshow("origin image", image_origin);
  cv::imshow("project image", image_project);
  cv::imwrite("/media/data/temp/image_origin.jpg", image_origin);
  cv::imwrite("/media/data/temp/image_project.jpg", image_project);
  cv::waitKey(10000);

  return 0;
}

后记：投影部分区域的点云到图像中，不要全部都投。（一般选取标定板所处位置的点云）