1. 原理
(1)确定所要投影的平面,将点云投影至该平面,得到二维点坐标;
(2)求得二维点云所在平面的极值,即x_max,x_min,y_max,y_min,;
(3)根据x_max - x_min,y_max - y_min确定点云范围;
(4)根据点云范围以及想要得到图像的分辨率的大小求得单个像素代表的实际长度L;
(5)遍历点云,将点的坐标与极小值点的坐标做差之后除以L,即为该点在图像中的像素坐标;
(6)将点的RGB信息填充到对应的像素内。
2. 代码
void pointcloud_to_image(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, float x_max, float x_min, float y_max, float y_min)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud->points.resize(cloud_in->size());
for (size_t i = 0; i < cloud->points.size(); i++) //将三维点云投影到二维,这里为投影到XOY平面
{
cloud->points[i].x = cloud_in->points[i].x;
cloud->points[i].y = cloud_in->points[i].y;
cloud->points[i].z = 0;
cloud->points[i].r = cloud_in->points[i].r;
cloud->points[i].g = cloud_in->points[i].g;
cloud->points[i].b = cloud_in->points[i].b;
}
Mat image(480, 640, CV_8UC3);
for (int j = 0; j < image.rows; j++)
{
for (int i = 0; i < image.cols; i++)
{
image.at<Vec3b>(j, i)[0] = 0;
image.at<Vec3b>(j, i)[1] = 0;
image.at<Vec3b>(j, i)[2] = 0;
}
}
float l; //单个像素代表的实际长度
float a = (x_max - x_min) / 640; //分辨率,根据实际需要设置,这里采用648*480
float b = (y_max - y_min) / 480;
if (a > b)
{
l = a;
}
else
{
l = b;
}
for (int i = 0; i < cloud->size(); i++)
{
//计算点对应的像素坐标
int x = (cloud->points[i].x - x_min) / l;
int y = (cloud->points[i].y - y_min) / l;
//将颜色信息赋予像素
if (x > 0 && x < 640 && y>0 && y < 480)
{
image.at<Vec3b>(y, x)[0] = cloud->points[i].r;
image.at<Vec3b>(y, x)[1] = cloud->points[i].g;
image.at<Vec3b>(y, x)[2] = cloud->points[i].b;
}
}
imshow("image", image);
waitKey(0);
}