 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Turning a PointCloud into an Image
Description: This tutorial is a simple guide to turning a dense point cloud into an image messageKeywords: kinect, pcl, opencv, openni
Tutorial Level: BEGINNER
Use ROS
Let's assume you have an openni camera up and running. Fire up a terminal.
$rostopic list /camera/depth/points2 /camera/rgb/camera_info /camera/rgb/image_color /camera/rgb/image_mono
To convert the point cloud to an image, just run the following
rosrun pcl_ros convert_pointcloud_to_image input:=/camera/depth/points2 output:=/camera/depth/cloud_image
Then subscribe to the image and display it
rosrun image_view image_view image:=/camera/depth/cloud_image
Concept
Given a dense cloud, you can efficiently grab the rgb image from it as a sensor_msgs::Image. The image message can then either be republished or turned into an opencv cv::Mat using standard methods
You may pass either pcl::PointCloud<PointT>, or a sensor_msgs::PointCloud2.
Code
See the pcl_ros/src/tools/convert_pointcloud_to_image and pcl_ros/src/tools/convert_pcd_to_image for example code.
1 // ROS core
2 #include <ros/ros.h>
3 //Image message
4 #include <sensor_msgs/Image.h>
5 //pcl::toROSMsg
6 #include <pcl/io/pcd_io.h>
7 //stl stuff
8 #include <string>
9
10 class PointCloudToImage
11 {
12 public:
13 void
14 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
15 {
16 if ((cloud->width * cloud->height) == 0)
17 return; //return if the cloud is not dense!
18 try
19 {
20 pcl::toROSMsg (*cloud, image_); //convert the cloud
21 }
22 catch (std::runtime_error e)
23 {
24 ROS_ERROR_STREAM("Error in converting cloud to image message: "
25 << e.what());
26 }
27 image_pub_.publish (image_); //publish our cloud image
28 }
29 PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
30 {
31 sub_ = nh_.subscribe (cloud_topic_, 30,
32 &PointCloudToImage::cloud_cb, this);
33 image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30);
34
35 //print some info about the node
36 std::string r_ct = nh_.resolveName (cloud_topic_);
37 std::string r_it = nh_.resolveName (image_topic_);
38 ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct );
39 ROS_INFO_STREAM("Publishing image on topic " << r_it );
40 }
41 private:
42 ros::NodeHandle nh_;
43 sensor_msgs::Image image_; //cache the image message
44 std::string cloud_topic_; //default input
45 std::string image_topic_; //default output
46 ros::Subscriber sub_; //cloud subscriber
47 ros::Publisher image_pub_; //image message publisher
48 };
49
50 int
51 main (int argc, char **argv)
52 {
53 ros::init (argc, argv, "convert_pointcloud_to_image");
54 PointCloudToImage pci; //this loads up the node
55 ros::spin (); //where she stops nobody knows
56 return 0;
57 }
