Ask Your Question
0

Data type conversion from PCL PointCloud <PointT> to ROS sensor_msgs/Image

asked 2015-09-12 03:20:08 -0500

Megacephalo gravatar image

I am doing RGBDSLAM with people filtering, i.e. I filtered out people from the point cloud and then output the remaining cluster for RGBDSLAM node to process. However, the RGBDSLAM node only subscribes to sensor_msgs/Image (rgb) and sensor_msgs/Image (depth). I tried to extract RGB and Depth messages directly from the pcl::PointCloud<pointt> before converting it into ROS sensor_msgs/Image. Here is part of my code in the cloud callback function:

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

PointCloudT::Ptr no_man_cloud (new PointCloudT) ;

sensor_msgs::Image rgbImage ;
sensor_msgs::Image depthImage ;

// RGB Topic
pcl::toROSMsg ( *no_man_cloud, rgbImage ) ;

// Depth Topic
pcl::PointCloud<pcl::PointXYZ> cloud_xyz ;
PointCloudT cloud_xyzrgb ;
pcl::copyPointCloud (*no_man_cloud, cloud_xyz) ;
pcl::copyPointCloud (cloud_xyz, cloud_xyzrgb) ;

pcl::toROSMsg ( cloud_xyzrgb, depthImage ) ;

rtabmap_rgbImage_pub.publish (rgbImage) ;
rtabmapDepthImage_pub.publish (depthImage) ;

It worked in theory, but actually renders nothing on the sensor_msgs/Image depth. Since I am outputting depth and rgb images out a a processed cloud, I cannot extract corresponding topics directly from the RGB-D sensor. Bearing this in mind, this article: How to convert pointcloud2 to separate depth and rgb image messages may not help me. Is there a proper way to output depth and rgb images from point cloud? Thanks !

edit retag flag offensive close merge delete

Comments

Hi @Megacephalo, I know it's fairly late, but could you please tell me how you resolved this issue? Cheers!

disha gravatar image disha  ( 2020-07-14 05:45:41 -0500 )edit

Hi @disha! Thanks for asking. I spent three to four days solving my own problem the hard way and still cannot figure it out completely. As the problem per se, I remember that five years ago, I did a work around to process the PointCloud in its sensor_msgs form. But as you brought this up again, I did try to solve it the way it should be solved. Although I did not get to a workable solution, I will give my take in the answer underneath.

Megacephalo gravatar image Megacephalo  ( 2020-07-20 02:13:38 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-07-20 02:46:12 -0500

Megacephalo gravatar image

updated 2020-07-20 03:01:02 -0500

In response to @disha 's request, I am sharing my insight to solve such problem, since it is not a trivial one and there isn't much reference to be found on the internet. In order to pull the RGB and depth sensor_msgs::Image topics out of PCL Pointcloud, the conversion involves a two-fold process.

As per the depth image conversion from PCL's PointCloud, we need to first convert PCL pointcloud to cv::Mat for depth image. Please refer to REP 118 for depth image format. Notice that the row and height convention of both libraries are opposite to each other [1].

Then, once we have the depth values expressed in cv::Mat, convert the OpenCV format to sensor_msgs::Image using cv_bridge refering to [2].

Where I got stuck is the fact that there is a lack of texts to back up a proper conversion from PointCloudXYZRGB to cv::Mat depth image.

Another alternative that I tried (in vain) was to convert the PCL PointCloud to its sensor_msgs::PointCloud2 counterpart. Since we are working with depth image, pcl::toROSMsg could do little since it works with RGB channels instead of depth. If we could figure out how sensor_msgs::Image store depth information and how to convert depth measurements from sensor::msgs::PointCloud2 to depth images, which involves transforming depth values from 32-bit floating point depth to mono8 integer encoding intensity pixels, this solution could be the way to go.

What I did in this regard was to reverse-engineer depth_image_proc and pcl::toROSMsg but so far haven't sorted out the last part of conversion but may serve as a hint if someone else wants to give it a try.

So there you have it. Maybe the above insight is already suitable to solve your problem or at least serves as a launching pad to explore it even further. Thank you for asking and keep safe and healthy!

Reference:

[1] [c++ - pcl::Pointcloud to cv::Mat depth image - Stack Overflow](https://stackoverflow.com/q...

[2] [Converting OpenCV images to ROS image messages](http://wiki.ros.org/cv_brid...

edit flag offensive delete link more

Comments

I tried the PCL PointCloud to sensor_msgs::PointCloud2 approach and this first conversion works fine using pcl::fromPCLPointCloud2(pcl, *cloud); however the second part produces an unordered vector (ie: a on dimensional vector and the height, width, encoding info is lost thus making conversion from sensor_msgs::Image to cv::Mat or cv_image in python impossible) pcl::toROSMsg(cloud, img);

Abhinavgandhi09 gravatar image Abhinavgandhi09  ( 2021-03-11 10:24:44 -0500 )edit

Do you have an example for the first method? I am also thinking of trying to convert Pointcloud2 to a numpy array and then convert that to a 2D array/ cv::Mat/ cv_image

Abhinavgandhi09 gravatar image Abhinavgandhi09  ( 2021-03-11 10:26:31 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-09-12 03:20:08 -0500

Seen: 886 times

Last updated: Jul 20 '20