Segmenter.h

Go to the documentation of this file.

#ifndef RAIL_SEGMENTATION_SEGMENTER_H_
#define RAIL_SEGMENTATION_SEGMENTER_H_

// RAIL Segmentation
#include "SegmentationZone.h"
#include "bounding_volume_calculator.h"

// ROS
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <rail_manipulation_msgs/SegmentedObjectList.h>
#include <rail_manipulation_msgs/SegmentObjects.h>
#include <rail_segmentation/RemoveObject.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <std_srvs/Empty.h>
#include <tf/transform_listener.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_ros/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// PCL
#include <pcl/common/common.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/segmentation/sac_segmentation.h>


// YAML
#include <yaml-cpp/yaml.h>

// BOOST
#include <boost/thread/mutex.hpp>

// C++ Standard Library
#include <fstream>
#include <string>

namespace rail
{
namespace segmentation
{

class Segmenter
{
public:
#if __cplusplus >= 201103L

  static constexpr bool DEFAULT_DEBUG = false;
  static constexpr double SAC_EPS_ANGLE = 0.15;
  static constexpr double SAC_DISTANCE_THRESHOLD = 0.01;
  static constexpr int SAC_MAX_ITERATIONS = 100;
  static constexpr double SURFACE_REMOVAL_PADDING = 0.005;
  static constexpr int DEFAULT_MIN_CLUSTER_SIZE = 200;
  static constexpr int DEFAULT_MAX_CLUSTER_SIZE = 10000;
  static constexpr double CLUSTER_TOLERANCE = 0.02;
  static constexpr double POINT_COLOR_THRESHOLD = 10;
  static constexpr double REGION_COLOR_THRESHOLD = 10;
  static constexpr float DOWNSAMPLE_LEAF_SIZE = 0.01;
  static constexpr double MARKER_SCALE = 0.01;
#else

  static const bool DEFAULT_DEBUG = false;
  static const double SAC_EPS_ANGLE = 0.15;
  static const double SAC_DISTANCE_THRESHOLD = 0.01;
  static const int SAC_MAX_ITERATIONS = 100;
  static const double SURFACE_REMOVAL_PADDING = 0.005;
  static const int DEFAULT_MIN_CLUSTER_SIZE = 200;
  static const int DEFAULT_MAX_CLUSTER_SIZE = 10000;
  static const double CLUSTER_TOLERANCE = 0.02;
  static const double POINT_COLOR_THRESHOLD = 10;
  static const double REGION_COLOR_THRESHOLD = 10;
  static const float DOWNSAMPLE_LEAF_SIZE = 0.01;
  static const double MARKER_SCALE = 0.01;
#endif

  Segmenter();

  bool okay() const;

private:
  void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);

  const SegmentationZone &getCurrentZone() const;

  bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
      rail_segmentation::RemoveObject::Response &res);

  bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

  bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

  bool segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res);

  bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects);

  rail_manipulation_msgs::SegmentedObject findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const;

  void extractClustersEuclidean(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      std::vector<pcl::PointIndices> &clusters) const;


  void extractClustersRGB(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
                          std::vector<pcl::PointIndices> &clusters) const;

  void inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions, const pcl::IndicesPtr &indices_out) const;

  void extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
      const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const;

  double averageZ(const std::vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const;

  visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const;

  sensor_msgs::Image createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
      const pcl::PointIndices &cluster) const;

  bool debug_, okay_, first_pc_in_, use_color_;
  int min_cluster_size_, max_cluster_size_;
  boost::mutex pc_mutex_, msg_mutex_;
  std::vector<SegmentationZone> zones_;

  ros::NodeHandle node_, private_node_;
  ros::ServiceServer segment_srv_, segment_objects_srv_, clear_srv_, remove_object_srv_;
  ros::Publisher segmented_objects_pub_, table_pub_, markers_pub_, table_marker_pub_, debug_pc_pub_, debug_img_pub_;
  ros::Subscriber point_cloud_sub_;
  tf::TransformListener tf_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf2_;

  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc_;
  rail_manipulation_msgs::SegmentedObjectList object_list_;
  rail_manipulation_msgs::SegmentedObject table_;
  visualization_msgs::MarkerArray markers_;
  visualization_msgs::Marker table_marker_;

};

}
}

Eigen::Vector3f RGB2Lab (const Eigen::Vector3f& colorRGB);

#endif