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.
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.