planar_removal.cpp

Go to the documentation of this file.

#include "planar_removal.h"

using namespace Eigen;

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

  //  cv::namedWindow(WINDOW);

  //string for port names

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

  // [init publishers]
  image_pub_ = it_.advertise("/planar_removal/image/dense", 1);
//  jei_pub_name = "/jump_edge/image_depth_jef";
//  this->jei_pub = this->it.advertise(jei_pub_name, 1);

  pcl2_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/planar_removal/pcl2/output", 15);
  pcl2_planes_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/planar_removal/pcl2/planes_output", 15);
  // Create a ROS publisher for the output point cloud
  coefficients_pub_ = nh_.advertise<pcl::ModelCoefficients> ("/planar_removal/ModelCoefficients/output", 1);

  // [init services]
  dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig>::CallbackType reconfig_callback_type = boost::bind(&PlanarRemoval::reconfigureCallback, this, _1, _2);

  dyn_reconfig_srv.setCallback(reconfig_callback_type); 

  // [init clients]
  
  // [init action servers]
  
  // [init action clients]

  ROS_INFO("starting planar_removal");

}

void PlanarRemoval::reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level)
{
  ROS_INFO("Reconfigure request : distance th: %f lasting_cloud: %f #planes: %d ",
           config.distance_threshold, config.cloud_remaining_proportion_threshold, config.max_num_of_planes_removed);

  mutex.enter();
  distance_threshold_ = config.distance_threshold;
  cloud_remaining_proportion_threshold_ = config.cloud_remaining_proportion_threshold;
  max_num_of_planes_removed_ = config.max_num_of_planes_removed;
  mutex.exit();
  
}

void PlanarRemoval::removePlanesXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> cloud,pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered_rgb, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered,pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes){ 

  pcl::PointCloud<pcl::PointXYZRGB> one_plane;
  pcl::PointCloud<pcl::PointXYZRGB> unstructured_cloud;

  pcl::copyPointCloud(cloud, cloud_filtered_rgb);
  pcl::copyPointCloud(cloud, cloud_filtered);

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices ());
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;

  double distance_threshold_aux;
  double cloud_remaining_proportion_threshold_aux;
  int max_num_of_planes_removed_aux; 

  mutex.enter();
  distance_threshold_aux = distance_threshold_;
  cloud_remaining_proportion_threshold_aux = cloud_remaining_proportion_threshold_;
  max_num_of_planes_removed_aux = max_num_of_planes_removed_; 
  mutex.exit();

  //init auxiliary point cloud
  cloud_planes.width = cloud.width;
  cloud_planes.height = cloud.height;
  cloud_planes.points.resize (one_plane.width * one_plane.height);
  cloud_planes.is_dense = cloud.is_dense;
  cloud_planes.header = cloud.header;

  // Optional
  seg.setOptimizeCoefficients (true);

  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (distance_threshold_aux);

  seg.setInputCloud (cloud.makeShared ());
  seg.segment (*inliers, *coefficients); 
  
  // Publish the model coefficients
  coefficients_pub_.publish (*coefficients);

// Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;

  int i=0, nr_points = (int) cloud.points.size ();
  // While 70% of the original cloud is still there
  while (cloud.points.size () > cloud_remaining_proportion_threshold_aux * nr_points && i < max_num_of_planes_removed_aux)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud.makeShared());
    seg.setMaxIterations (1000);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0) {
      ROS_INFO("Could not estimate a planar model for the given dataset.");
      break;
    }

    // Extract the inliers
    extract.setInputCloud (cloud.makeShared());
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (one_plane);

    cloud_planes += one_plane;

    // Create the filtering object
    //extract.setNegative (true);
    //extract.filter (cloud_filtered);
//    pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter_extract_xyzrgb = extract.getIndices().begin();
    for(int j = 0; j < inliers->indices.size(); j++){
        uint8_t r = 0, g = 0, b = 0;    // Example: black color
        uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
        cloud_filtered_rgb[inliers->indices[j]].rgb = *reinterpret_cast<float*>(&rgb); 
        cloud_filtered[inliers->indices[j]].x = std::numeric_limits<float>::quiet_NaN();
        cloud_filtered[inliers->indices[j]].y = std::numeric_limits<float>::quiet_NaN(); 
        cloud_filtered[inliers->indices[j]].z = std::numeric_limits<float>::quiet_NaN(); 
    } 
    i++;
  }

}

// [subscriber callbacks]
void PlanarRemoval::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_);

  removePlanesXYZRGB(pcl_xyzrgb_, pcl_xyzrgb_aux_rgb, pcl_xyzrgb_aux_nan, pcl_planes_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_aux_rgb,rgb_image);

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

  pcl::toROSMsg(pcl_xyzrgb_aux_nan, pcl2_filtered_msg_);
  // [publish Point Cloud 2 and Depth Image]
  pcl2_pub_.publish(pcl2_filtered_msg_);
  pcl::toROSMsg(pcl_planes_xyzrgb_, pcl2_planes_msg_);
  pcl2_planes_pub_.publish(pcl2_planes_msg_);

}

void PlanarRemoval::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");
  PlanarRemoval planar_removal;
  ros::spin();
  return 0;
}