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/ProcessSegmentedObjects.h>
#include <rail_manipulation_msgs/SegmentedObjectList.h>
#include <rail_manipulation_msgs/SegmentObjects.h>
#include <rail_manipulation_msgs/SegmentObjectsFromPointCloud.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:
    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 calculateFeaturesCallback(rail_manipulation_msgs::ProcessSegmentedObjects::Request &req,
        rail_manipulation_msgs::ProcessSegmentedObjects::Response &res);

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

    bool segmentObjectsFromPointCloudCallback(rail_manipulation_msgs::SegmentObjectsFromPointCloud::Request &req, rail_manipulation_msgs::SegmentObjectsFromPointCloud::Response &res);

    bool segmentObjects(rail_manipulation_msgs::SegmentedObjectList &objects);

    bool executeSegmentation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
        rail_manipulation_msgs::SegmentedObjectList &objects);

    bool findSurface(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &in, const pcl::IndicesConstPtr &indices_in,
        const SegmentationZone &zone, const pcl::IndicesPtr &indices_out,
        rail_manipulation_msgs::SegmentedObject &table_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_, use_color_;
    int min_cluster_size_, max_cluster_size_;
    boost::mutex msg_mutex_;
    std::vector<SegmentationZone> zones_;
    bool crop_first_;
    bool label_markers_;
    double cluster_tolerance_;

    ros::NodeHandle node_, private_node_;
    ros::ServiceServer segment_srv_, segment_objects_srv_, segment_objects_from_point_cloud_srv_, clear_srv_, remove_object_srv_, calculate_features_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_;

    std::string point_cloud_topic_;

    rail_manipulation_msgs::SegmentedObjectList object_list_;
    rail_manipulation_msgs::SegmentedObject table_;
    visualization_msgs::MarkerArray markers_;
    visualization_msgs::MarkerArray text_markers_;
    visualization_msgs::Marker table_marker_;

};

}
}

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

#endif