pcl_to_image.cpp

Go to the documentation of this file.

#include "pcl_to_image.h"

using namespace Eigen;

// [Constructor]
PCLtoImage::PCLtoImage() : it_(nh_) {

  //  cv::namedWindow(WINDOW);

  //string for port names

  // [init subscribers]
  pcl2_sub_ = this->nh_.subscribe<sensor_msgs::PointCloud2>("input", 1, &PCLtoImage::pcl2_sub_callback, this);

  // [init publishers]
  image_pub_ = it_.advertise("output_image", 1);

  ROS_INFO("Starting PCL to Image");

}


// [subscriber callbacks]
void PCLtoImage::pcl2_sub_callback(const sensor_msgs::PointCloud2::ConstPtr& pcl2_msg_) { 

  // Create the filtering object: downsample the dataset using a leaf size of 1cm
//  pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
//  sor.setInputCloud (*pcl2_msg_);
//  sor.setLeafSize (0.01f, 0.01f, 0.01f);
//  sor.filter(*pcl2_msg_);

  cv::Mat rgb_image;
  pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_aux_rgb;
  pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_aux_nan;

  pcl::fromROSMsg(*pcl2_msg_, pcl_xyzrgb_);

  //rgb_image.create(pcl_xyzrgb_aux.height, pcl_xyzrgb_aux.width, CV_8UC3); 

  cv_image.header.stamp = ros::Time::now();
  cv_image.header.frame_id = "camera";
  cv_image.encoding = "bgr8";

  rgb_image = cv::Mat::zeros(rgb_image.size(), CV_8UC3);

  image_from_sparse_pcl2(pcl_xyzrgb_,rgb_image);

  cv_image.image = rgb_image;
  
  image_pub_.publish(cv_image.toImageMsg());
}

void PCLtoImage::image_from_sparse_pcl2(pcl::PointCloud<pcl::PointXYZRGB> cloud, cv::Mat& rgb_image){

  pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_xyzrgb = cloud.begin();

  rgb_image.create(cloud.height, cloud.width, CV_8UC3); 

  if(cloud.height != rgb_image.rows || cloud.width != rgb_image.cols){
    ROS_ERROR("Size mismatch between point cloud and rgb_image");
    ROS_INFO("xyzrgb size %d %d data size %d xyz %d %d data size %d rgb_image %d %d",pcl_xyzrgb_.height, pcl_xyzrgb_.width, pcl_xyzrgb_.size(),cloud.height, cloud.width, cloud.size(), rgb_image.rows, rgb_image.cols);
  } else {
    for (int rr = 0; rr < cloud.height; ++rr) {
      for (int cc = 0; cc < cloud.width; ++cc, ++pt_iter_xyzrgb) {
        pcl::PointXYZRGB &pt_xyzrgb = *pt_iter_xyzrgb;
        RGBValue color;
        color.float_value = pt_xyzrgb.rgb;
        if(isnan(pt_iter_xyzrgb->x) && isnan(pt_iter_xyzrgb->y) && isnan(pt_iter_xyzrgb->z)){
            rgb_image.at<cv::Vec3b>(rr,cc)[0] = 0;
            rgb_image.at<cv::Vec3b>(rr,cc)[1] = 0;
            rgb_image.at<cv::Vec3b>(rr,cc)[2] = 0; 
        }else{
            rgb_image.at<cv::Vec3b>(rr,cc)[0] = color.Blue;
            rgb_image.at<cv::Vec3b>(rr,cc)[1] = color.Green;
            rgb_image.at<cv::Vec3b>(rr,cc)[2] = color.Red;
        }
      }
    }
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "planar_removal");
  PCLtoImage planar_removal;
  ros::spin();
  return 0;
}