Segmenter.h
Go to the documentation of this file.
1 
13 #ifndef RAIL_SEGMENTATION_SEGMENTER_H_
14 #define RAIL_SEGMENTATION_SEGMENTER_H_
15 
16 // RAIL Segmentation
17 #include "SegmentationZone.h"
19 
20 // ROS
21 #include <pcl_ros/point_cloud.h>
22 #include <pcl_ros/transforms.h>
23 #include <rail_manipulation_msgs/SegmentedObjectList.h>
24 #include <rail_manipulation_msgs/SegmentObjects.h>
25 #include <rail_segmentation/RemoveObject.h>
26 #include <ros/package.h>
27 #include <ros/ros.h>
28 #include <sensor_msgs/Image.h>
30 #include <sensor_msgs/PointCloud2.h>
32 #include <std_srvs/Empty.h>
33 #include <tf/transform_listener.h>
36 #include <visualization_msgs/Marker.h>
37 #include <visualization_msgs/MarkerArray.h>
38 
39 // PCL
40 #include <pcl/common/common.h>
41 #include <pcl/filters/conditional_removal.h>
42 #include <pcl/filters/extract_indices.h>
43 #include <pcl/filters/passthrough.h>
44 #include <pcl/filters/project_inliers.h>
45 #include <pcl/filters/voxel_grid.h>
46 #include <pcl/ModelCoefficients.h>
47 #include <pcl/point_types.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/segmentation/extract_clusters.h>
50 #include <pcl/segmentation/region_growing_rgb.h>
51 #include <pcl/segmentation/sac_segmentation.h>
52 
53 
54 // YAML
55 #include <yaml-cpp/yaml.h>
56 
57 // BOOST
58 #include <boost/thread/mutex.hpp>
59 
60 // C++ Standard Library
61 #include <fstream>
62 #include <string>
63 
64 namespace rail
65 {
66 namespace segmentation
67 {
68 
76 class Segmenter
77 {
78 public:
79 #if __cplusplus >= 201103L
80 
81  static constexpr bool DEFAULT_DEBUG = false;
83  static constexpr double SAC_EPS_ANGLE = 0.15;
85  static constexpr double SAC_DISTANCE_THRESHOLD = 0.01;
87  static constexpr int SAC_MAX_ITERATIONS = 100;
89  static constexpr double SURFACE_REMOVAL_PADDING = 0.005;
91  static constexpr int DEFAULT_MIN_CLUSTER_SIZE = 200;
93  static constexpr int DEFAULT_MAX_CLUSTER_SIZE = 10000;
95  static constexpr double CLUSTER_TOLERANCE = 0.02;
97  static constexpr double POINT_COLOR_THRESHOLD = 10;
99  static constexpr double REGION_COLOR_THRESHOLD = 10;
101  static constexpr float DOWNSAMPLE_LEAF_SIZE = 0.01;
103  static constexpr double MARKER_SCALE = 0.01;
104 #else
105 
106  static const bool DEFAULT_DEBUG = false;
108  static const double SAC_EPS_ANGLE = 0.15;
110  static const double SAC_DISTANCE_THRESHOLD = 0.01;
112  static const int SAC_MAX_ITERATIONS = 100;
114  static const double SURFACE_REMOVAL_PADDING = 0.005;
116  static const int DEFAULT_MIN_CLUSTER_SIZE = 200;
118  static const int DEFAULT_MAX_CLUSTER_SIZE = 10000;
120  static const double CLUSTER_TOLERANCE = 0.02;
122  static const double POINT_COLOR_THRESHOLD = 10;
124  static const double REGION_COLOR_THRESHOLD = 10;
126  static const float DOWNSAMPLE_LEAF_SIZE = 0.01;
128  static const double MARKER_SCALE = 0.01;
129 #endif
130 
136  Segmenter();
137 
145  bool okay() const;
146 
147 private:
155  void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);
156 
166  const SegmentationZone &getCurrentZone() const;
167 
178  bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
179  rail_segmentation::RemoveObject::Response &res);
180 
191  bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
192 
203  bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
204 
215  bool segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res);
216 
225  bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects);
226 
241  rail_manipulation_msgs::SegmentedObject findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
242  const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const;
243 
253  void extractClustersEuclidean(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
254  std::vector<pcl::PointIndices> &clusters) const;
255 
256 
266  void extractClustersRGB(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
267  std::vector<pcl::PointIndices> &clusters) const;
268 
279  void inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
280  const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions, const pcl::IndicesPtr &indices_out) const;
281 
291  void extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
292  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const;
293 
302  double averageZ(const std::vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const;
303 
312  visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const;
313 
323  sensor_msgs::Image createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
324  const pcl::PointIndices &cluster) const;
325 
331  boost::mutex pc_mutex_, msg_mutex_;
333  std::vector<SegmentationZone> zones_;
334 
349 
351  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc_;
353  rail_manipulation_msgs::SegmentedObjectList object_list_;
355  rail_manipulation_msgs::SegmentedObject table_;
357  visualization_msgs::MarkerArray markers_;
359  visualization_msgs::Marker table_marker_;
360 
361 };
362 
363 }
364 }
365 
366 Eigen::Vector3f RGB2Lab (const Eigen::Vector3f& colorRGB);
367 
368 #endif
const SegmentationZone & getCurrentZone() const
Determine the current zone based on the latest state of the TF tree.
Definition: Segmenter.cpp:264
bool segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res)
Callback for the main segmentation request.
Definition: Segmenter.cpp:350
static const double POINT_COLOR_THRESHOLD
Definition: Segmenter.h:122
ros::ServiceServer remove_object_srv_
Definition: Segmenter.h:338
visualization_msgs::Marker table_marker_
Definition: Segmenter.h:359
rail_manipulation_msgs::SegmentedObjectList object_list_
Definition: Segmenter.h:353
void extract(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &out) const
Extract a new point cloud based on the given indices.
Definition: Segmenter.cpp:999
ros::Publisher debug_img_pub_
Definition: Segmenter.h:340
static const double SAC_EPS_ANGLE
Definition: Segmenter.h:108
ros::ServiceServer segment_objects_srv_
Definition: Segmenter.h:338
bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback for the main segmentation request.
Definition: Segmenter.cpp:344
static const int DEFAULT_MAX_CLUSTER_SIZE
Definition: Segmenter.h:118
tf::TransformListener tf_
Definition: Segmenter.h:344
pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr pc_
Definition: Segmenter.h:351
ros::NodeHandle private_node_
Definition: Segmenter.h:336
The main grasp collector node object.
Definition: Segmenter.h:76
void extractClustersRGB(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
Find clusters in a point cloud.
Definition: Segmenter.cpp:848
bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback for the clear request.
Definition: Segmenter.cpp:319
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
Find and remove a surface from the given point cloud.
Definition: Segmenter.cpp:647
static const float DOWNSAMPLE_LEAF_SIZE
Definition: Segmenter.h:126
static const bool DEFAULT_DEBUG
Definition: Segmenter.h:106
bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects)
Callback for the main segmentation request.
Definition: Segmenter.cpp:356
visualization_msgs::MarkerArray markers_
Definition: Segmenter.h:357
static const double CLUSTER_TOLERANCE
Definition: Segmenter.h:120
static const double SURFACE_REMOVAL_PADDING
Definition: Segmenter.h:114
Segmenter()
Create a Segmenter and associated ROS information.
Definition: Segmenter.cpp:24
bool okay() const
A check for a valid Segmenter.
Definition: Segmenter.cpp:250
std::vector< SegmentationZone > zones_
Definition: Segmenter.h:333
bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req, rail_segmentation::RemoveObject::Response &res)
Callback for the remove object request.
Definition: Segmenter.cpp:291
void pointCloudCallback(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &pc)
Callback for the point cloud topic.
Definition: Segmenter.cpp:255
static const int SAC_MAX_ITERATIONS
Definition: Segmenter.h:112
ros::Publisher markers_pub_
Definition: Segmenter.h:340
double averageZ(const std::vector< pcl::PointXYZRGB, Eigen::aligned_allocator< pcl::PointXYZRGB > > &v) const
Find the average Z value of the point vector.
Definition: Segmenter.cpp:1024
ros::Publisher segmented_objects_pub_
Definition: Segmenter.h:340
static const int DEFAULT_MIN_CLUSTER_SIZE
Definition: Segmenter.h:116
tf2_ros::TransformListener tf2_
Definition: Segmenter.h:348
The criteria for a segmentation zone.
static const double REGION_COLOR_THRESHOLD
Definition: Segmenter.h:124
static const double MARKER_SCALE
Definition: Segmenter.h:128
ros::Publisher table_marker_pub_
Definition: Segmenter.h:340
static const double SAC_DISTANCE_THRESHOLD
Definition: Segmenter.h:110
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
Bound a point cloud based on the inverse of a set of conditions.
Definition: Segmenter.cpp:1008
Eigen::Vector3f RGB2Lab(const Eigen::Vector3f &colorRGB)
Definition: Segmenter.cpp:1035
visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const
Create a Marker from the given point cloud.
Definition: Segmenter.cpp:937
ros::Subscriber point_cloud_sub_
Definition: Segmenter.h:342
ros::Publisher debug_pc_pub_
Definition: Segmenter.h:340
rail_manipulation_msgs::SegmentedObject table_
Definition: Segmenter.h:355
ros::ServiceServer segment_srv_
Definition: Segmenter.h:338
The criteria for a segmentation zone.
ros::ServiceServer clear_srv_
Definition: Segmenter.h:338
void extractClustersEuclidean(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::IndicesConstPtr &indices_in, std::vector< pcl::PointIndices > &clusters) const
Find clusters in a point cloud.
Definition: Segmenter.cpp:822
tf2_ros::Buffer tf_buffer_
Definition: Segmenter.h:346
sensor_msgs::Image createImage(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &in, const pcl::PointIndices &cluster) const
Create a cropped image of the segmented object.
Definition: Segmenter.cpp:875


rail_segmentation
Author(s): Russell Toris , David Kent
autogenerated on Sun Jun 30 2019 19:58:20