How to convert cv::Mat to pcl::pointcloud with color

Question

I am trying to convert a pointcloud with rgb information from pcl format to cv::Mat and back to pcl. I have found convert mat to pointcloud on stackoverflow.

Code Updated

I therefore used that code found on stackoverflow as a starting point. And now have the following:

//input pcl::PointCloud<pcl::PointXYZRGB> point_cloud_ptr
cv::Mat OpenCVPointCloud(3, point_cloud_ptr.points.size(), CV_64FC1);
OpenCVPointCloudColor = cv::Mat(480, 640, CV_8UC3);
for(int y=0;y<OpenCVPointCloudColor.rows;y++)
{
    for(int x=0;x<OpenCVPointCloudColor.cols;x++)
    {
        OpenCVPointCloud.at<double>(0,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).x;
        OpenCVPointCloud.at<double>(1,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).y;
        OpenCVPointCloud.at<double>(2,y*OpenCVPointCloudColor.cols+x) = point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).z;

        cv::Vec3b color = cv::Vec3b(point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).b,point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).g,point_cloud_ptr.points.at(y*OpenCVPointCloudColor.cols+x).r);

        OpenCVPointCloudColor.at<cv::Vec3b>(cv::Point(x,y)) = color;
    }
}

cv::imshow("view 2", OpenCVPointCloudColor);
cv::waitKey(30);

Displaying the image above ensured me that the colours are extracted correctly (the image is compared by eye with the raw image). I then want to convert it back to a pointcloud:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);

for(int y=0;y<OpenCVPointCloudColor.rows;y++)
{
    for(int x=0;x<OpenCVPointCloudColor.cols;x++)
    {
        pcl::PointXYZRGB point;
        point.x = OpencVPointCloud.at<double>(0,y*OpenCVPointCloudColor.cols+x);
        point.y = OpencVPointCloud.at<double>(1,y*OpenCVPointCloudColor.cols+x);
        point.z = OpencVPointCloud.at<double>(2,y*OpenCVPointCloudColor.cols+x);

            cv::Vec3b color = OpenCVPointCloudColor.at<cv::Vec3b>(cv::Point(x,y));
        //Try 1 not working
        //uint8_t r = (color[2]); 
        //uint8_t g = (color[1]);
        //uint8_t b = (color[0]);

        //Try 2 not working
        //point.r = color[2];
        //point.g = color[1];
        //point.b = color[0];

        //Try 3 not working
        //uint8_t r = (color.val[2]);
        //uint8_t g = (color.val[1]);
        //uint8_t b = (color.val[0]);

        //test working WHY DO THE ABOVE CODES NOT WORK :/
        uint8_t r = 255; 
        uint8_t g = 0;
        uint8_t b = 0;
        int32_t rgb = (r << 16) | (g << 8) | b;
        point.rgb = *reinterpret_cast<float*>(&rgb);

        point_cloud_ptr -> points.push_back(point);
    }
}

Can anybody explain why the values explicitly specified 255,0,0 colours everything red. But if I choose the output from colour image, the cloud is wrongly coloured?

Stumbled upon this, I cannot see what difference my code has other than then the cloud type is different?

update

reading this discussion on PCL ended with me switching to xyzrgba (also mentioned on stackoverflow). The code I then tried when converting back is:

point.b = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[0];
point.g = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[1];
point.r = OpenCVPointCloudColor.at<cv::Vec3b>(x,y)[2];
point.a = 255;

The resulting colour cloud is different from the ones created in XYZRGB, but are still wrong :/ WTF?

Extra

Even if I force a red colour into all points OpenCVPointCloudColor using cvCloudColor.at<cv::Vec3f>(x,y) = cv::Vec3f(0,0,255); then reading from OpenCVPointCloudColor still creates wrong colour information in the pcl cloud.

In the example you seem to store the points in `OpenCVPointCloudColor` while attempting to read them from `colorImage`. Are those the same? — Morris Franken, Jan 24 '17 at 00:22
@iamai yes they are, I will update the question now so they have the same variable names. (just had them in two function calls). — JTIM, Jan 24 '17 at 00:37
(irrelevant from the code above) If you are trying to read them from an image loaded using OpenCV, beware that the default color system is BGR instead of RGB. That might affect the colors. — ilke444, Jan 24 '17 at 04:06
@like444 yes that is right. But that should not have any effect on the code above? — JTIM, Jan 24 '17 at 12:35

score 1 · Accepted Answer · answered Jan 27 '17 at 01:54

I am almost sure that there is a bug somewhere in your code around these functions. I tried simple example, following your paradigm, and it works perfectly (PCL 1.8 built from sources, OpenCV 3.1 built from sources, g++ 5.x or g++ 6.x, Ubuntu 16.10):

#include <pcl/visualization/cloud_viewer.h>

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

void draw_cloud(
    const std::string &text,
    const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud)
{
    pcl::visualization::CloudViewer viewer(text);
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())
    {
    }
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr img_to_cloud(
        const cv::Mat& image,
        const cv::Mat &coords)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

    for (int y=0;y<image.rows;y++)
    {
        for (int x=0;x<image.cols;x++)
        {
            pcl::PointXYZRGB point;
            point.x = coords.at<double>(0,y*image.cols+x);
            point.y = coords.at<double>(1,y*image.cols+x);
            point.z = coords.at<double>(2,y*image.cols+x);

            cv::Vec3b color = image.at<cv::Vec3b>(cv::Point(x,y));
            uint8_t r = (color[2]);
            uint8_t g = (color[1]);
            uint8_t b = (color[0]);

            int32_t rgb = (r << 16) | (g << 8) | b;
            point.rgb = *reinterpret_cast<float*>(&rgb);

            cloud->points.push_back(point);
        }
    }
    return cloud;
}

void cloud_to_img(
        const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
        cv::Mat &coords,
        cv::Mat &image)
{
    coords = cv::Mat(3, cloud->points.size(), CV_64FC1);
    image = cv::Mat(480, 640, CV_8UC3);
    for(int y=0;y<image.rows;y++)
    {
        for(int x=0;x<image.cols;x++)
        {
            coords.at<double>(0,y*image.cols+x) = cloud->points.at(y*image.cols+x).x;
            coords.at<double>(1,y*image.cols+x) = cloud->points.at(y*image.cols+x).y;
            coords.at<double>(2,y*image.cols+x) = cloud->points.at(y*image.cols+x).z;

            cv::Vec3b color = cv::Vec3b(
                    cloud->points.at(y*image.cols+x).b,
                    cloud->points.at(y*image.cols+x).g,
                    cloud->points.at(y*image.cols+x).r);

            image.at<cv::Vec3b>(cv::Point(x,y)) = color;
        }
    }
}

int main(int argc, char *argv[])
{
    cv::Mat image = cv::imread("test.png");
    cv::resize(image, image, cv::Size(640, 480));
    cv::imshow("initial", image);

    cv::Mat coords(3, image.cols * image.rows, CV_64FC1);
    for (int col = 0; col < coords.cols; ++col)
    {
        coords.at<double>(0, col) = col % image.cols;
        coords.at<double>(1, col) = col / image.cols;
        coords.at<double>(2, col) = 10;
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = img_to_cloud(image, coords);
    draw_cloud("points", cloud);

    cloud_to_img(cloud, coords, image);
    cv::imshow("returned", image);

    cv::waitKey();
    return 0;
}

"initial" and "returned" images are totally the same. In PCL's viewer I see my image as point cloud, of course, with z = 10, because I hardcoded it. You possibly should use mouse wheel to zoom out and see entire image.

For running this example you should have file named 'test.png' in your working directory.

I am sorry for hardcoded input file name and resize to fixed resolution.

Try it, and if it works in your system, try to find bug in your code. If it does not work, possibly your PCL build is too old or even broken.

Thank you I will try this tomorrow. I however have (pcl 1.7, Ubuntu 14.04, ROS indigo full, i.e. opencv 2.4). Had multiple issues with pcl 1.8 and opencv 3.1 and ROS. I was able to run Pcl nodes and opencv nodes but nothing in conjunction for some odd reason. So this was new fresh install. Nevertheless thanks I'll test your code to see if I have mad a blunder. — JTIM, Jan 27 '17 at 02:26
Something must be broken in my configuration. Thank you for clarifying it. I will mark it as solved by you — JTIM, Jan 27 '17 at 16:00

How to convert cv::Mat to pcl::pointcloud with color

1 Answers1