点云转二维图

开发过程中，有将三维点云转换为二维图片的需求。介绍2个转换的方式：

一、通过三维平面投影到二维图片上，这种方式相对准确。

假设有一个相机坐标系下的三维点(x,y,)，则它在图像平面上的投影坐标(u,v )可通过以下公式计算得到:

u= (fx *X +cx*Z) / Z

v= (fy *Y +cy*Z) / Z

其中，fx、fy、cx、y表示相机的内部参数，1表示单位矩阵的最后一个元素.

相机内参矩阵K：

fx 0 cx

0 fy cy

0 0 1

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);    
    double fx = 4810;
    double fy = 4807.4;
    double cx = 0;
    double cy = 0;
    double minValue = std::min(min.x, min.y);
    minValue = -minValue;        //整体平移，防止出现负数
     // Convert point cloud to depth image
    cv::Mat depth_image(cloud->width(), cloud->height() , CV_32FCI);
    float* depth_data = (float*)depth_image.data;
    for (int i = 0; i < cloud->points.size(); i++) {
        pcl::PointXYZRGB point = cloud->points[i];
        int u = std::round((point.x+ minValue) / fabs(point.z) * fx + cx);
        int v = std::round((point.y+ minValue) / fabs(point.z) * fy + cy);
        if (u >= 0 && u < depth_image.cols && v >= 0 && v < depth_image.rows) {
            depth_data[v * depth_image.cols + u] = fabs(point.z);
        }
    }
    double min_depth , max_depth ;
    cv::minMaxLoc(depth_image, &min_depth, &max_depth);
    cv::Mat normalized_depth_image;
//z轴颜色归一至0-255
    cv::normalize(depth_image, normalized_depth_image, 0, 255, cv::NORM_MINMAX, CV_32FC1);
    // 将归一化后的深度图像数据类型转换为CV_8UC1类型
    cv::Mat depth_image_8u;
    normalized_depth_image.convertTo(depth_image_8u, CV_8UC1);
    cv::Mat colorDepthImage; // 上色后的深度图像
    // 将灰度深度图映射为热度图
    cv::applyColorMap(depth_image_8u, colorDepthImage, cv::COLORMAP_JET);
    cv::imshow("Depth Image", colorDepthImage);

注意：以上的点云是针对有序的点云数据，如果点云是无序的（即height为1），图片的大小可用点云最值代替pcl::getMinMax3D(*cloud, min, max)不再赘述。

二、根据点云位置点直接对应到二维图片，可能会失真（不得不用才使用）

点云根据点云最大最小或者点云宽高在图片像素上遍历。

    //获取点云最值
    pcl::PointXYZRGB min;//用于存放三个轴的最小值
    pcl::PointXYZRGB max;//用于存放三个轴的最大值
    pcl::getMinMax3D(*cloud, min, max);
    for (auto& point : cloud->points) {//转为正数
        point.x -= min.x;
        point.y -= min.y;
        point.z -= min.z;
    }
    pcl::getMinMax3D(*cloud, min, max);
    int Prow = ceil(max.x - min.x) ;//向上取整
    int Pcol = ceil(max.y - min.y) ;
    cv::Mat image1(Prow,Pcol,  CV_8UC1);//注意当前为单通道，灰色
    //cv::Mat image(image1.rows, image1.cols, CV_8UC3, 
    //cv::Scalar(255, 255, 255)); 转为三通道

    //初始化图像像素为255
    for (int i = 0; i < image.rows; i++)
    {
        for (int j = 0; j < image.cols; j++)
        {
            image.at<uchar>(j,i) = 255;
        }
    }
    //根据点云高度对图像进行赋值
    for (int i = 0; i < cloud1->points.size(); i++)
    {
        int image_i, image_j;
    
        image_i = cloud1->points[i].y;
        image_j =cloud1->points[i].x;
        //根据点云高度，进行0-255转换
        image1.at<uchar>(image_i, image_j) = uchar((cloud1->points[i].z - min.z) * 255 / (max.z - min.z));
        
        //cv::Vec3b pixel;
        //pixel[0] = cloud->points[i].b;  // blue
        //pixel[1] = cloud->points[i].g;    // green
        //pixel[2] = cloud->points[i].r;    // red
        //image.at<cv::Vec3b>(image_i, image_j) = pixel;
     }
    cv::Mat color_image;
    cv::applyColorMap(image1, color_image, cv::COLORMAP_JET);    //伪彩图
    imshow("Point Cloud Image", color_image);

注意：图片格式类型。

总结：图片处理生成需要多了解图片的一些原理，比如深度值是点云z值最值跨度，是二维图像的灰度值等。

---

更新：有时候会出现点云和图片不一致导致图片只有轮廓的情况，这时我们需要对点云值和图片大小进行缩放，使得所有信息能够全部显示。

    float scale = 0.1;    //缩小
     pcl::getMinMax3D(*inputCloud, min, max);
    int Prow = ceil(max.x - min.x) ;//向上取整
    int Pcol = ceil(max.y - min.y) ;
    cv::Mat image(Pcol * scale, Prow * scale, CV_8UC3, cv::Scalar(0, 0, 0));

    //初始化图像像素为255
    for (int i = 0; i < image.rows; i++)
    {
        for (int j = 0; j < image.cols; j++)
        {
            image.at<uchar>(i, j) = 0;
        }
    }

    for(int index = 0; index < inputCloud->points.size(); ++index)
    {
        inputCloud->points[index].x -= topLeft.x;
        inputCloud->points[index].y -= topLeft.y;
        inputCloud->points[index].z -= topLeft.z;

        int image_i, image_j;
        image_i = inputCloud->points[index].x * scale;
        image_j = inputCloud->points[index].y * scale;
        cv::Vec3b pixel;
        pixel[0] = inputCloud->points[index].b;  // blue
        pixel[1] = inputCloud->points[index].g;  // green
        pixel[2] = inputCloud->points[index].r;  // red
        if (image_j< image.rows && image_i < image.cols)
        {
            image.at<cv::Vec3b>(image_j, image_i) = pixel;
        }
    }
   cv::imwrite("MyCloud.jpg", image);