iros12_planar_removal.h

Go to the documentation of this file.

#ifndef _PLANAR_REMOVAL_IROS2012_H_
#define _PLANAR_REMOVAL_IROS2012_H_

// [Eigen]
#include "Eigen/Core"

// [ROS]
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "image_transport/image_transport.h" 
#include <sensor_msgs/point_cloud_conversion.h>

// [PCL_ROS]
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ros/register_point_struct.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/conditional_removal.h>

#include <cmath>

//pcl::toROSMsg
#include <pcl/io/pcd_io.h>

//iriutils
#include "mutex.h"

// [OpenCV]
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/highgui/highgui.hpp>

// [publisher subscriber headers]
#include "sensor_msgs/Image.h"
#include "sensor_msgs/PointCloud2.h"

// [services]
#include <dynamic_reconfigure/server.h>
#include <iri_pcl_filters/IriPclFiltersParametersConfig.h>

// static const char WINDOW[] = "Image window";

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

class ConditionColor:public pcl::ComparisonBase<pcl::PointXYZRGB> {
  RGBValue mean_;
  float std_;
public:
  ConditionColor(RGBValue the_mean, float the_std) : pcl::ComparisonBase<pcl::PointXYZRGB>()
  {
    //deafault should be:
    //this->mean_.R=143.4074;
    //this->mean_.G=134.5007;
    //this->mean_.B=134.8869;
    //this->std_=28.3;
    this->mean_=the_mean;
    this->std_=the_std;
    capable_=true;
  }
  
  bool evaluate(const pcl::PointXYZRGB &point) const
  {
    RGBValue color;
    color.float_value = point.rgb;
    float dist = sqrt(pow(color.Red - this->mean_.Red, 2)+
                      pow(color.Blue - this->mean_.Blue, 2)+
                      pow(color.Green - this->mean_.Green, 2));
    if(dist<2*this->std_)
      return false;
    else
      return true;
  }
  //~ConditionBase(){}
  //~ConditionColor(){}
};

class PlanarRemovalIROS2012 {
    public:
      PlanarRemovalIROS2012();
      ~PlanarRemovalIROS2012();
     
    private:

      double distance_threshold_;
      double cloud_remaining_proportion_threshold_;
      int max_num_of_planes_removed_; 

      CMutex mutex;

      cv_bridge::CvImage cv_image;

      sensor_msgs::Image image_msg_;
      
      ros::NodeHandle nh_;

      pcl::PointCloud<pcl::PointXYZ> pcl_xyz_;
      pcl::PointCloud<pcl::PointXYZRGB> pcl_xyzrgb_;
      pcl::PointCloud<pcl::PointXYZ> pcl_xyzrgb2_;
      pcl::PointCloud<pcl::PointXYZ> pcl_planes_xyz_;
      pcl::PointCloud<pcl::PointXYZRGB> pcl_planes_xyzrgb_;

      void removePlanesXYZ(pcl::PointCloud<pcl::PointXYZ> cloud, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered, pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes );
      void removePlanesXYZRGB(pcl::PointCloud<pcl::PointXYZRGB> cloud, pcl::PointCloud<pcl::PointXYZRGB>& cloud_filtered, pcl::PointCloud<pcl::PointXYZRGB>& cloud_planes );
      void image_from_pcl2(cv::Mat& rgb_image);
      void image_from_sparse_pcl2(pcl::PointCloud<pcl::PointXYZRGB> cloud, cv::Mat& rgb_image);

      // [Subscribers]
      ros::Subscriber pcl2_sub_;

      // [Publishers]
      ros::Publisher pcl2_pub_;
      ros::Publisher pcl2_planes_pub_;
      ros::Publisher coefficients_pub_;
      image_transport::ImageTransport it_; 

      sensor_msgs::PointCloud2 pcl2_filtered_msg_;
      sensor_msgs::PointCloud2 pcl2_planes_msg_;

      image_transport::Publisher image_pub_;
  
      dynamic_reconfigure::Server<iri_pcl_filters::IriPclFiltersParametersConfig> dyn_reconfig_srv;
      
      void reconfigureCallback(iri_pcl_filters::IriPclFiltersParametersConfig &config, uint32_t level);

      void pcl2_sub_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
};
#endif