rgbd2cloud.cpp

Go to the documentation of this file.

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>
#include <ros/ros.h>

//this is a global definition of the points to be used
//changes to omit color would need adaptations in 
//the visualization too
#include <pcl/io/io.h>
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
namespace sm = sensor_msgs;
namespace mf = message_filters;
typedef pcl::PointXYZRGB point_type;
typedef pcl::PointCloud<point_type> pointcloud_type;
typedef mf::sync_policies::ApproximateTime<sm::Image, sm::Image, sm::CameraInfo> NoCloudSyncPolicy;

typedef union
{
  struct /*anonymous*/
  {
    unsigned char Blue;
    unsigned char Green;
    unsigned char Red;
    unsigned char Alpha;
  };
  float float_value;
  long long_value;
} RGBValue;

pointcloud_type* createPointCloud (const sm::ImageConstPtr& depth_msg, 
                                   const sm::ImageConstPtr& rgb_msg,
                                   const sm::CameraInfoConstPtr& cam_info) 
{
  pointcloud_type* cloud (new pointcloud_type() );
  cloud->header.stamp     = depth_msg->header.stamp;
  cloud->header.frame_id  = depth_msg->header.frame_id;
  cloud->is_dense         = true; //single point of view, 2d rasterized

  float cx, cy, fx, fy;//principal point and focal lengths
  unsigned color_step = 0, color_skip = 0;

  cloud->height = depth_msg->height;
  cloud->width = depth_msg->width;
  cx = cam_info->K[2]; //(cloud->width >> 1) - 0.5f;
  cy = cam_info->K[5]; //(cloud->height >> 1) - 0.5f;
  fx = 1.0f / cam_info->K[0]; 
  fy = 1.0f / cam_info->K[4]; 
  int pixel_data_size = 3;
  char red_idx = 0, green_idx = 1, blue_idx = 2;

  if(rgb_msg->encoding.compare("mono8") == 0) pixel_data_size = 1;
  if(rgb_msg->encoding.compare("bgr8") == 0) { red_idx = 2; blue_idx = 0; }

  ROS_ERROR_COND(pixel_data_size == 0, "Unknown image encoding: %s!", rgb_msg->encoding.c_str());
  color_step = pixel_data_size * rgb_msg->width / cloud->width;
  color_skip = pixel_data_size * (rgb_msg->height / cloud->height - 1) * rgb_msg->width;

  cloud->points.resize (cloud->height * cloud->width);

  const float* depth_buffer = reinterpret_cast<const float*>(&depth_msg->data[0]);
  const uint8_t* rgb_buffer = &rgb_msg->data[0];

  // depth_msg already has the desired dimensions, but rgb_msg may be higher res.
  int color_idx = 0, depth_idx = 0;

  pointcloud_type::iterator pt_iter = cloud->begin ();
  for (int v = 0; v < (int)cloud->height; ++v, color_idx += color_skip)
  {
    for (int u = 0; u < (int)cloud->width; ++u, color_idx += color_step, ++depth_idx, ++pt_iter)
    {
      point_type& pt = *pt_iter;
      float Z = depth_buffer[depth_idx];

      // Check for invalid measurements
      if (std::isnan (Z))
      {
        pt.x = pt.y = pt.z = Z;
      }
      else // Fill in XYZ
      {
        pt.x = (u - cx) * Z * fx;
        pt.y = (v - cy) * Z * fy;
        pt.z = Z;
      }

      if(rgb_msg.get() != 0){
        // Fill in color
        RGBValue color;
        if(pixel_data_size == 3){
          color.Red   = rgb_buffer[color_idx + red_idx];
          color.Green = rgb_buffer[color_idx + green_idx];
          color.Blue  = rgb_buffer[color_idx + blue_idx];
        } else {
          color.Red   = color.Green = color.Blue  = rgb_buffer[color_idx];
        }
        color.Alpha = 0;
        pt.rgb = color.float_value;
      }
    }
  }

  return cloud;
}


void rgbd_callback(const sm::ImageConstPtr& rgbimage,
                   const sm::ImageConstPtr& dimage, 
                   const sm::CameraInfoConstPtr& cam_info,
                   ros::Publisher* cloud_pub)
{
  pointcloud_type* pc = createPointCloud(dimage, rgbimage, cam_info);
  sm::PointCloud2 cloudMessage;
  pcl::toROSMsg(*pc,cloudMessage);
  cloud_pub->publish(cloudMessage);
  delete pc;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "rgbd2cloud");
  ros::NodeHandle nh;
  ros::Publisher pc_pub = nh.advertise<sm::PointCloud2>("cloud_out", 10);
  ROS_INFO("To reconstruct point clouds from openni images call this tool as follows:\nrosrun rgbd2cloud rgbd2cloud rgb_image:=/camera/rgb/image_color depth_image:=/camera/depth/image camera_info:=/camera/depth/camera_info");

  //Subscribers
  mf::Subscriber<sm::Image> rgb_sub(nh, "rgb_image", 2);
  mf::Subscriber<sm::Image> depth_sub(nh, "depth_image", 2);
  mf::Subscriber<sm::CameraInfo> info_sub(nh, "camera_info", 5);

  //Syncronize rgb, depth and calibration
  mf::Synchronizer<NoCloudSyncPolicy> no_cloud_sync(NoCloudSyncPolicy(2), rgb_sub, depth_sub, info_sub);
  no_cloud_sync.registerCallback(boost::bind(&rgbd_callback, _1, _2, _3, &pc_pub));

  ros::spin();
  return 0;
}