37 #ifndef JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_ 38 #define JSK_PCL_ROS_LINE_SEGMENT_DETECTOR_H_ 40 #include <pcl/segmentation/sac_segmentation.h> 49 #include <jsk_recognition_msgs/ClusterPointIndices.h> 50 #include <jsk_recognition_msgs/ModelCoefficientsArray.h> 51 #include <jsk_pcl_ros/LineSegmentDetectorConfig.h> 52 #include <dynamic_reconfigure/server.h> 53 #include <visualization_msgs/Marker.h> 64 pcl::PointIndices::Ptr indices,
65 pcl::ModelCoefficients::Ptr coefficients,
66 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
68 pcl::ModelCoefficients::Ptr coefficients);
71 visualization_msgs::Marker& marker,
72 const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
73 double minimum_line_length);
84 pcl::PointCloud<pcl::PointXYZ>::Ptr
points_;
102 sensor_msgs::PointCloud2,
105 sensor_msgs::PointCloud2,
108 typedef jsk_pcl_ros::LineSegmentDetectorConfig
Config;
114 virtual void unsubscribe();
115 virtual void onInit();
116 virtual void segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
117 const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& cluster_msg);
118 virtual void configCallback(Config &config, uint32_t level);
119 virtual void segmentLines(
120 const pcl::PointCloud<PointT>::Ptr& cloud,
121 const pcl::PointIndices::Ptr& indices,
122 std::vector<pcl::PointIndices::Ptr>& line_indices,
123 std::vector<pcl::ModelCoefficients::Ptr>& line_coefficients);
124 virtual void publishResult(
125 const std_msgs::Header&
header,
126 const pcl::PointCloud<PointT>::Ptr& cloud,
127 const std::vector<LineSegment::Ptr>& segments);
152 pcl::SACSegmentation<PointT>
seg_;
pcl::PointCloud< pcl::PointXYZ >::Ptr points_
double outlier_threshold_
ros::Publisher pub_coefficients_
pcl::PointCloud< pcl::PointXYZ >::Ptr getRawPoints()
jsk_pcl_ros::LineSegmentDetectorConfig Config
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > SyncPolicy
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
boost::recursive_mutex config_mutex_
pcl::ModelCoefficients::Ptr coefficients_
virtual jsk_recognition_utils::Line::Ptr toSegment()
pcl::PointCloud< pcl::PointXYZ >::Ptr raw_points_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
pcl::ModelCoefficients::Ptr getCoefficients()
pcl::PointCloud< pcl::PointXYZ >::Ptr getPoints()
boost::mutex mutex
global mutex.
pcl::PointIndices::Ptr getIndices()
pcl::PointIndices::Ptr indices_
ros::Publisher pub_line_marker_
virtual bool addMarkerLine(visualization_msgs::Marker &marker, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double minimum_line_length)
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy
pcl::SACSegmentation< PointT > seg_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
LineSegment(const std_msgs::Header &input_header, pcl::PointIndices::Ptr indices, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
boost::shared_ptr< LineSegment > Ptr
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
ros::Publisher pub_indices_