PCL三维点云中的立体框映射到二维图像（在图像中绘制立体框）

---

前言

三维点云与图像的映射，或者说是将三维点云中的点与二维图像中的某一个像素点进行对应。那么这么做的目的是什么呢？用途是什么呢“
目的：目的就是将三维点与二维图像之间的映射关系
作用：1、可以给点云赋予彩色信息，增强点云所表达物体或对象的辨识度；2、可以将三维点云中绘制的目标物体通过映射关系绘制到二维图像中（最基本的就是三维立体框的绘制），这个工作在点云标注邻域被广泛使用。3、可以根据点云中绘制的结果提取二维图像中对应的物体。

一、二维图像与点云映射

要实现将点云中的点或者是立体框绘制到二维图像中，首先就需要找到点云与二维图像之间的映射关系（将点云与二维像素点对应），有关点云与二维图像之间的映射过程可以看博客《激光雷达点云与单幅图像配准/映射变为彩色点云》。该文中给出了详细的转换过程、转换源码和免费的测试文件，有需要的可自行下载和测试。

二、将点云中的点或立体框映射到二维图像中

这个过程其实并不难理解，就是通过点云与图像的映射关系将三维点（x,y,z）转换为二维像素点（u,v），然后再根据二维像素点进行图像绘制即可。
首先：考虑到点云中部分图像的点可能会超出原图像的大小，所有我们对图像边缘进行扩大，绘制结束后再进行裁剪。

	cv::Mat maskImage = cv::Mat(image.rows + 500, image.cols+500, CV_8UC3, cv::Scalar(255, 255, 255));;
	cv::Mat img1 = maskImage(cv::Range(0, image.rows), cv::Range(0, image.cols));
	image.copyTo(img1);

三维点与二维限速之间的映射过程，注意:具体的映射过程要看博客《激光雷达点云与单幅图像配准/映射变为彩色点云》

calculate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<cv::Point> &outPixel) {
	cv::Mat word_h = cv::Mat(4, 1, CV_64FC1);
	cv::Mat p_result = cv::Mat(3, 1, CV_64FC1);
	cv::Point xy;
	for (int i = 0; i < cloud->size(); i++) {
		word_h = (cv::Mat_<double>(4, 1) << cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, 1);
		p_result = K * tc * word_h;
		int u = (int)((p_result.at<double>(0, 0)) / p_result.at<double>(2, 0));
		int v = (int)((p_result.at<double>(1, 0)) / p_result.at<double>(2, 0));
		if (u >= 0 && u < cols && v >= 0 && v < rows) {
			xy.y = v;
			xy.x = u;
			outPixel.push_back(xy);
		}
	}

点云坐标绘制到二维图像源码

drawBox(std::vector<pcl::PointCloud<pcl::PointXYZ>>allRangeBOX_3D_PointList) {
	//cv::Mat img;
	//cv::undistort(image, UndistortImage, K1, D, K1);
	cv::Mat maskImage = cv::Mat(image.rows + 500, image.cols+500, CV_8UC3, cv::Scalar(255, 255, 255));;
	cv::Mat img1 = maskImage(cv::Range(0, image.rows), cv::Range(0, image.cols));
	image.copyTo(img1);
	UndistortImage = maskImage;
	cols = UndistortImage.cols;
	rows = UndistortImage.rows;
	for (int i = 0; i < allRangeBOX_3D_PointList.size(); i++) {
		if (allRangeBOX_3D_PointList[i].size() == 0) continue;
		std::vector<cv::Point> outPixel;
		calculate(allRangeBOX_3D_PointList[i].makeShared(), outPixel);
		std::cout << "outPixel.size()" << outPixel.size() << endl;
		if (outPixel.size() < 8)continue;// 判断二维像素点的个数，因为立体框有八个点
		cv::Scalar color = cv::Scalar(255., 0., 0.);
		cv::line(UndistortImage, outPixel[0], outPixel[1], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[0], outPixel[3], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[0], outPixel[4], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[4], outPixel[5], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[4], outPixel[7], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[1], outPixel[5], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[5], outPixel[6], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[6], outPixel[7], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[1], outPixel[2], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[3], outPixel[7], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[2], outPixel[3], color, 5, cv::LINE_8);
		cv::line(UndistortImage, outPixel[2], outPixel[6], color, 5, cv::LINE_8);
	}
	UndistortImage = UndistortImage(cv::Range(0, image.rows), cv::Range(0, image.cols));
	cols = UndistortImage.cols;
	rows = UndistortImage.rows;
}

测试结果