iros12_planar_removal.cpp

Go to the documentation of this file.

#include "iros12_planar_removal.h"

using namespace Eigen;

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

  //  cv::namedWindow(WINDOW);

  //string for port names

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

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

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

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

  dyn_reconfig_srv.setCallback(reconfig_callback_type); 

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

  ROS_INFO("starting planar_removal_iros2012");

}


void PlanarRemovalIROS2012::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();
  
}

PlanarRemovalIROS2012::~PlanarRemovalIROS2012(){
  //  cv::destroyWindow(WINDOW);
}

void PlanarRemovalIROS2012::removePlanesXYZ(pcl::PointCloud<pcl::PointXYZ> cloud,pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered,pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes){ 

  pcl::PointCloud<pcl::PointXYZ> one_plane;
  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::PointXYZ> seg;

  double distance_threshold_aux;
  double cloud_remaining_proportion_threshold_aux;

  mutex.enter();
  distance_threshold_aux = distance_threshold_;
  cloud_remaining_proportion_threshold_aux = cloud_remaining_proportion_threshold_;
  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);

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

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZ> extract;
      
  // Extract the inliers
  extract.setInputCloud (cloud.makeShared());
  extract.setIndices (inliers);
  extract.setNegative (false);
  extract.filter (one_plane);
  //cloud_planes += one_plane;
  pcl::copyPointCloud(one_plane, cloud_planes); 

  ROS_INFO("points ok %d",inliers->indices.size());
  for(int j = 0; j < inliers->indices.size(); j++){
    uint8_t r = 255, g = 0, b = 0;    // Example: Red color
    uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
    cloud_filtered[inliers->indices[j]].rgb = *reinterpret_cast<float*>(&rgb); 
  }
  //generate mask image
  cv::Mat mask(480,640,CV_8UC1);
  mask.setTo(255);

  int z=0;
  uint8_t r = 255, g = 0, b = 0;    // Example: Red color
  uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
  
  for(int j=0;j<mask.rows;j++)
  {
    for(int i=0;i<mask.cols;i++)
    {
      if(cloud_filtered[z].rgb == *reinterpret_cast<float*>(&rgb) || isnan(cloud_filtered[z].z))
        mask.ptr(j)[i]=(uint)0;
      z++;
    }
  }
  cv::imwrite("/tmp/tamp.png",mask);
}


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

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

  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;

  mutex.enter();
  distance_threshold_aux = distance_threshold_;
  cloud_remaining_proportion_threshold_aux = cloud_remaining_proportion_threshold_;
  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;

  if(0)
  {
    RGBValue color_accept;
    color_accept.Red=143.4074;
    color_accept.Green=134.5007;
    color_accept.Blue=134.8869;
    
    pcl::ComparisonBase<pcl::PointXYZRGB>::Ptr color_filter(new ConditionColor(color_accept, 28.3));
    pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr andfilter(new pcl::ConditionAnd<pcl::PointXYZRGB>);
    andfilter->addComparison(color_filter);
    pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem (andfilter);
    
    condrem.setInputCloud(cloud.makeShared());
    condrem.setKeepOrganized(true);
    pcl::PointIndicesPtr inliers3(new pcl::PointIndices);
    for (int i=0;i<cloud.width*cloud.height;i++)
      inliers3->indices.push_back(i);
    std::cout<<" s0120941093 "<< inliers3->indices.size();
    condrem.setIndices(inliers3);
    pcl::IndicesConstPtr inliers2;
    inliers2=condrem.getRemovedIndices();
    condrem.filter(color_valid_points);

    for(int i=0;i<inliers2->size();i++)
    {
      std::cout<<(*inliers2)[i]<<" ";
      inliers->indices.push_back((*inliers2)[i]);
    }
  }else{
  // 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;

    
  // seg.setOptimizeCoefficients (false);
  // seg.setInputCloud (cloud.makeShared());
  // seg.segment(*inliers, *coefficients);
  // std::cout<<"----"<<*coefficients<<std::endl;
  if (inliers->indices.size () == 0) {
    ROS_INFO("Could not estimate a planar model for the given dataset.");
    return;
  }
      
  // Extract the inliers
  extract.setInputCloud (cloud.makeShared());
  extract.setIndices (inliers);
  extract.setNegative (false);
  extract.filter (one_plane);
  
  cloud_planes += one_plane;

  ROS_INFO("points ok %d",inliers->indices.size());
  for(int j = 0; j < inliers->indices.size(); j++){
    uint8_t r = 255, g = 0, b = 0;    // Example: Red color
    uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
    cloud_filtered[inliers->indices[j]].rgb = *reinterpret_cast<float*>(&rgb); 
  }
  //generate mask image
  cv::Mat mask(480,640,CV_8UC1);
  mask.setTo(255);

  int z=0;
  uint8_t r = 255, g = 0, b = 0;    // Example: Red color
  uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
  
  for(int j=0;j<mask.rows;j++)
  {
    for(int i=0;i<mask.cols;i++)
    {
      if(cloud_filtered[z].rgb == *reinterpret_cast<float*>(&rgb) )
        mask.ptr(j)[i]=(uint)0;
      z++;
    }
  }
  cv::imwrite("/tmp/tamp.png",mask);
}

// [subscriber callbacks]
void PlanarRemovalIROS2012::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;
  pcl::PointCloud<pcl::PointXYZ> pcl_xyz_aux;

  
  if(sensor_msgs::getPointCloud2FieldIndex(*pcl2_msg_,"rgb")!=-1)
  {
    pcl::fromROSMsg(*pcl2_msg_, pcl_xyzrgb_);
    removePlanesXYZRGB(pcl_xyzrgb_, pcl_xyzrgb_aux, pcl_planes_xyzrgb_);
  }
  else
  {
    pcl::fromROSMsg(*pcl2_msg_, pcl_xyz_);
    removePlanesXYZ(pcl_xyz_, pcl_xyzrgb_aux, pcl_planes_xyzrgb_);
  }
  
  //ROS_INFO("5 msg size %d %d data size %d size %d %d data size %d rgb_image %d %d",pcl2_msg_->height, pcl2_msg_->width, pcl2_msg_->data.size(),pcl_xyzrgb_aux.height, pcl_xyzrgb_aux.width, pcl_xyzrgb_aux.size(), rgb_image.rows, rgb_image.cols);

  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::imread("/home/pmonso/default.jpg");
  rgb_image = cv::Mat::zeros(rgb_image.size(), CV_8UC3);
  image_from_sparse_pcl2(pcl_xyzrgb_aux,rgb_image);
  cv_image.image = rgb_image;
  
  image_pub_.publish(cv_image.toImageMsg());

  pcl::toROSMsg(pcl_xyzrgb_, pcl2_filtered_msg_);
  // [publish Point Cloud 2 and Depth Image]
  pcl2_pub_.publish(pcl2_filtered_msg_);
//  this->image_pub.publish(image_msg_);  //  this->jei_pub.publish(Image_msg_);
  pcl::toROSMsg(pcl_planes_xyzrgb_, pcl2_planes_msg_);
  pcl2_planes_pub_.publish(pcl2_planes_msg_);

}

void PlanarRemovalIROS2012::image_from_pcl2(cv::Mat& rgb_image){

//  pcl::PointCloud<pcl::PointXYZRGB>::iterator pt_iter = this->pcl_xyzrgb_.begin();
//
//  rgb_image.create(this->pcl_xyzrgb_.height, this->pcl_xyzrgb_.width, CV_8UC3);
//  for (int rr = 0; rr < (int)this->pcl_xyzrgb_.height; ++rr) {
//    for (int cc = 0; cc < (int)this->pcl_xyzrgb_.width; ++cc, ++pt_iter) {
//      pcl::PointXYZRGB &pt = *pt_iter;
//      RGBValue color;
//      color.float_value = pt.rgb;
//      rgb_image.at<Vec3b>(rr,cc)[0] = color.Blue;
//      rgb_image.at<Vec3b>(rr,cc)[1] = color.Green;
//      rgb_image.at<Vec3b>(rr,cc)[2] = color.Red;
//    }
//  }
}

void PlanarRemovalIROS2012::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(this->pcl_xyz_.height, this->pcl_xyz_.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_iros2012");
  PlanarRemovalIROS2012 planar_removal;
  ros::spin();
  return 0;
}