pcl::Pointcloud to cv::Mat depth image

Question

I am converting this depth image to a pcl::pointcloud.

using the following:

PointCloud::Ptr PointcloudUtils::RGBDtoPCL(cv::Mat depth_image, Eigen::Matrix3f& _intrinsics)
{
    PointCloud::Ptr pointcloud(new PointCloud);

    float fx = _intrinsics(0, 0);
    float fy = _intrinsics(1, 1);
    float cx = _intrinsics(0, 2);
    float cy = _intrinsics(1, 2);

    float factor = 1;

    depth_image.convertTo(depth_image, CV_32F); // convert the image data to float type 

    if (!depth_image.data) {
        std::cerr << "No depth data!!!" << std::endl;
        exit(EXIT_FAILURE);
    }

    pointcloud->width = depth_image.cols; //Dimensions must be initialized to use 2-D indexing 
    pointcloud->height = depth_image.rows;
    pointcloud->resize(pointcloud->width*pointcloud->height);

#pragma omp parallel for
    for (int v = 0; v < depth_image.rows; v += 4)
    {
        for (int u = 0; u < depth_image.cols; u += 4)
        {
            float Z = depth_image.at<float>(v, u) / factor;

            PointT p;
            p.z = Z;
            p.x = (u - cx) * Z / fx;
            p.y = (v - cy) * Z / fy;

            p.z = p.z / 1000;
            p.x = p.x / 1000;
            p.y = p.y / 1000;

            pointcloud->points.push_back(p);

        }
    }

    return pointcloud;

}

this works great, I have run some processing on the cloud, and now I need to convert the pointcloud back to a cv::Mat depth image. I cannot find an example for this, and am having trouble getting m head around it. What is the opposite of the above function?

How can i convert a pcl::pointcloud back to a cv::mat?

Thank you.

Do you want the p.x, p.y,p.z to represent one float pixel? — zindarod
Hi, thank you for your reply. I think so, yes. Basically I want to end up with the image above. Same number of cols / rows. — anti
... The goal is to reverse the above function. So to go from image to pointcloud, then back to exactly the same image. Thanks! — anti
Editing the code. I've a question. Why are you incrementing the loop variables by 4? — zindarod
Ah sorry, that was to test downsampling. i have replaced it with ++ i am trying your new code, and it looks as though it should work! But I still get a black image. I am testing various settings to try and nail it down. thank you very much for your help. — anti

zindarod zindarod · Accepted Answer · 2017-10-12T20:29:48

This is untested code, since I don't have point cloud on my machine. From your own conversion code I am assuming your image a single channel image.

void PCL2Mat(PointCloud::Ptr pointcloud, cv::Mat& depth_image, int original_width, int original_height)
{
    if (!depth_image.empty())
        depth_image.release();

    depth_image.create(original_height, original_width, CV_32F);

    int count = 0;

    #pragma omp parallel for
    for (int v = 0; v < depth_image.rows; ++v)
    {
        for (int u = 0; u < depth_image.cols; ++u)
        {
            depth_image.at<float>(v, u) = pointcloud->points.at(count++).z * 1000;

        }
    }

    depth_image.convertTo(depth_image,CV_8U);

}

pcl::Pointcloud to cv::Mat depth image

2 Answers