## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0= 
## descriptive title for the tutorial
## title = Turning a PointCloud into an Image
## multi-line description to be displayed in search 
## description = This tutorial is a simple guide to turning a dense point cloud into an image message
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= BeginnerCategory
## keywords = kinect, pcl, opencv, openni
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>
= 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 [[http://www.ros.org/wiki/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages | standard methods]]
{{{
#!cplusplus
//pcl::toROSMsg
#include <pcl/io/pcd_io.h>
//...
    try
    {
      pcl::toROSMsg (*cloud, image_); //convert the cloud
    }
    catch (std::runtime_error)
    {
    }
}}}

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.

{{{
#!cplusplus
// ROS core
#include <ros/ros.h>
//Image message
#include <sensor_msgs/Image.h>
//pcl::toROSMsg
#include <pcl/io/pcd_io.h>
//stl stuff
#include <string>

class PointCloudToImage
{
public:
  void
  cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud)
  {
    if ((cloud->width * cloud->height) == 0)
      return; //return if the cloud is not dense!
    try
    {
      pcl::toROSMsg (*cloud, image_); //convert the cloud
    }
    catch (std::runtime_error e)
    {
      ROS_ERROR_STREAM("Error in converting cloud to image message: "
                        << e.what());
    }
    image_pub_.publish (image_); //publish our cloud image
  }
  PointCloudToImage () : cloud_topic_("input"),image_topic_("output")
  {
    sub_ = nh_.subscribe (cloud_topic_, 30,
                          &PointCloudToImage::cloud_cb, this);
    image_pub_ = nh_.advertise<sensor_msgs::Image> (image_topic_, 30);

    //print some info about the node
    std::string r_ct = nh_.resolveName (cloud_topic_);
    std::string r_it = nh_.resolveName (image_topic_);
    ROS_INFO_STREAM("Listening for incoming data on topic " << r_ct );
    ROS_INFO_STREAM("Publishing image on topic " << r_it );
  }
private:
  ros::NodeHandle nh_;
  sensor_msgs::Image image_; //cache the image message
  std::string cloud_topic_; //default input
  std::string image_topic_; //default output
  ros::Subscriber sub_; //cloud subscriber
  ros::Publisher image_pub_; //image message publisher
};

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "convert_pointcloud_to_image");
  PointCloudToImage pci; //this loads up the node
  ros::spin (); //where she stops nobody knows
  return 0;
}
}}}


## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE