Go to the documentation of this file.00001
00013 #ifndef RAIL_SEGMENTATION_SEGMENTER_H_
00014 #define RAIL_SEGMENTATION_SEGMENTER_H_
00015
00016
00017 #include "SegmentationZone.h"
00018 #include "bounding_volume_calculator.h"
00019
00020
00021 #include <pcl_ros/point_cloud.h>
00022 #include <pcl_ros/transforms.h>
00023 #include <rail_manipulation_msgs/SegmentedObjectList.h>
00024 #include <rail_manipulation_msgs/SegmentObjects.h>
00025 #include <rail_segmentation/RemoveObject.h>
00026 #include <ros/package.h>
00027 #include <ros/ros.h>
00028 #include <sensor_msgs/Image.h>
00029 #include <sensor_msgs/image_encodings.h>
00030 #include <sensor_msgs/PointCloud2.h>
00031 #include <sensor_msgs/point_cloud_conversion.h>
00032 #include <std_srvs/Empty.h>
00033 #include <tf/transform_listener.h>
00034 #include <tf2/LinearMath/Matrix3x3.h>
00035 #include <tf2_ros/transform_listener.h>
00036 #include <visualization_msgs/Marker.h>
00037 #include <visualization_msgs/MarkerArray.h>
00038
00039
00040 #include <pcl/common/common.h>
00041 #include <pcl/filters/conditional_removal.h>
00042 #include <pcl/filters/extract_indices.h>
00043 #include <pcl/filters/passthrough.h>
00044 #include <pcl/filters/project_inliers.h>
00045 #include <pcl/filters/voxel_grid.h>
00046 #include <pcl/ModelCoefficients.h>
00047 #include <pcl/point_types.h>
00048 #include <pcl/point_cloud.h>
00049 #include <pcl/segmentation/extract_clusters.h>
00050 #include <pcl/segmentation/region_growing_rgb.h>
00051 #include <pcl/segmentation/sac_segmentation.h>
00052
00053
00054
00055 #include <yaml-cpp/yaml.h>
00056
00057
00058 #include <boost/thread/mutex.hpp>
00059
00060
00061 #include <fstream>
00062 #include <string>
00063
00064 namespace rail
00065 {
00066 namespace segmentation
00067 {
00068
00076 class Segmenter
00077 {
00078 public:
00079 #if __cplusplus >= 201103L
00080
00081 static constexpr bool DEFAULT_DEBUG = false;
00083 static constexpr double SAC_EPS_ANGLE = 0.15;
00085 static constexpr double SAC_DISTANCE_THRESHOLD = 0.01;
00087 static constexpr int SAC_MAX_ITERATIONS = 100;
00089 static constexpr double SURFACE_REMOVAL_PADDING = 0.005;
00091 static constexpr int DEFAULT_MIN_CLUSTER_SIZE = 200;
00093 static constexpr int DEFAULT_MAX_CLUSTER_SIZE = 10000;
00095 static constexpr double CLUSTER_TOLERANCE = 0.02;
00097 static constexpr double POINT_COLOR_THRESHOLD = 10;
00099 static constexpr double REGION_COLOR_THRESHOLD = 10;
00101 static constexpr float DOWNSAMPLE_LEAF_SIZE = 0.01;
00103 static constexpr double MARKER_SCALE = 0.01;
00104 #else
00105
00106 static const bool DEFAULT_DEBUG = false;
00108 static const double SAC_EPS_ANGLE = 0.15;
00110 static const double SAC_DISTANCE_THRESHOLD = 0.01;
00112 static const int SAC_MAX_ITERATIONS = 100;
00114 static const double SURFACE_REMOVAL_PADDING = 0.005;
00116 static const int DEFAULT_MIN_CLUSTER_SIZE = 200;
00118 static const int DEFAULT_MAX_CLUSTER_SIZE = 10000;
00120 static const double CLUSTER_TOLERANCE = 0.02;
00122 static const double POINT_COLOR_THRESHOLD = 10;
00124 static const double REGION_COLOR_THRESHOLD = 10;
00126 static const float DOWNSAMPLE_LEAF_SIZE = 0.01;
00128 static const double MARKER_SCALE = 0.01;
00129 #endif
00130
00136 Segmenter();
00137
00145 bool okay() const;
00146
00147 private:
00155 void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &pc);
00156
00166 const SegmentationZone &getCurrentZone() const;
00167
00178 bool removeObjectCallback(rail_segmentation::RemoveObject::Request &req,
00179 rail_segmentation::RemoveObject::Response &res);
00180
00191 bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00192
00203 bool segmentCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00204
00215 bool segmentObjectsCallback(rail_manipulation_msgs::SegmentObjects::Request &req, rail_manipulation_msgs::SegmentObjects::Response &res);
00216
00225 bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects);
00226
00241 rail_manipulation_msgs::SegmentedObject findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00242 const SegmentationZone &zone, const pcl::IndicesPtr &indices_out) const;
00243
00253 void extractClustersEuclidean(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00254 std::vector<pcl::PointIndices> &clusters) const;
00255
00256
00266 void extractClustersRGB(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00267 std::vector<pcl::PointIndices> &clusters) const;
00268
00279 void inverseBound(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00280 const pcl::ConditionBase<pcl::PointXYZRGB>::Ptr &conditions, const pcl::IndicesPtr &indices_out) const;
00281
00291 void extract(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
00292 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &out) const;
00293
00302 double averageZ(const std::vector<pcl::PointXYZRGB, Eigen::aligned_allocator<pcl::PointXYZRGB> > &v) const;
00303
00312 visualization_msgs::Marker createMarker(const pcl::PCLPointCloud2::ConstPtr &pc) const;
00313
00323 sensor_msgs::Image createImage(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in,
00324 const pcl::PointIndices &cluster) const;
00325
00327 bool debug_, okay_, first_pc_in_, use_color_;
00329 int min_cluster_size_, max_cluster_size_;
00331 boost::mutex pc_mutex_, msg_mutex_;
00333 std::vector<SegmentationZone> zones_;
00334
00336 ros::NodeHandle node_, private_node_;
00338 ros::ServiceServer segment_srv_, segment_objects_srv_, clear_srv_, remove_object_srv_;
00340 ros::Publisher segmented_objects_pub_, table_pub_, markers_pub_, table_marker_pub_, debug_pc_pub_, debug_img_pub_;
00342 ros::Subscriber point_cloud_sub_;
00344 tf::TransformListener tf_;
00346 tf2_ros::Buffer tf_buffer_;
00348 tf2_ros::TransformListener tf2_;
00349
00351 pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc_;
00353 rail_manipulation_msgs::SegmentedObjectList object_list_;
00355 rail_manipulation_msgs::SegmentedObject table_;
00357 visualization_msgs::MarkerArray markers_;
00359 visualization_msgs::Marker table_marker_;
00360
00361 };
00362
00363 }
00364 }
00365
00366 Eigen::Vector3f RGB2Lab (const Eigen::Vector3f& colorRGB);
00367
00368 #endif