37 #ifndef JSK_PCL_ROS_EDGE_DEPTH_REFINEMENT_H_ 38 #define JSK_PCL_ROS_EDGE_DEPTH_REFINEMENT_H_ 41 #include <sensor_msgs/PointCloud2.h> 42 #include <jsk_recognition_msgs/ClusterPointIndices.h> 43 #include <jsk_recognition_msgs/ModelCoefficientsArray.h> 44 #include <jsk_recognition_msgs/SegmentArray.h> 52 #include <jsk_pcl_ros/EdgeDepthRefinementConfig.h> 53 #include <dynamic_reconfigure/server.h> 54 #include <boost/tuple/tuple.hpp> 64 sensor_msgs::PointCloud2,
67 typedef jsk_pcl_ros::EdgeDepthRefinementConfig
Config;
75 const sensor_msgs::PointCloud2ConstPtr &
point,
76 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices);
79 const pcl::PointCloud<PointT>::Ptr& cloud,
80 const std::vector<int>& indices,
81 pcl::PointIndices& inliers,
82 pcl::ModelCoefficients& coefficients);
85 const pcl::PointCloud<PointT>::Ptr& cloud,
86 const std::vector<PCLIndicesMsg>& indices,
87 std::vector<pcl::PointIndices::Ptr>& output_inliers,
88 std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients);
91 const pcl::PointCloud<PointT>::Ptr& cloud,
92 const std::vector<pcl::PointIndices::Ptr> inliers,
93 const std::vector<pcl::ModelCoefficients::Ptr> coefficients,
94 std::vector<pcl::PointIndices::Ptr>& output_inliers,
95 std::vector<pcl::ModelCoefficients::Ptr>& output_coefficients);
98 const pcl::ModelCoefficients::Ptr coefficients);
101 const pcl::PointCloud<PointT>::Ptr& cloud,
102 const std::vector<int>& indices,
109 const std::vector<pcl::PointIndices::Ptr> inliers,
110 const std::vector<pcl::ModelCoefficients::Ptr> coefficients,
111 const std_msgs::Header&
header);
114 const int width,
const int height,
115 const std::vector<int>& indices);
118 const pcl::PointCloud<PointT>::Ptr& cloud,
119 const std::set<int>& duplicated_set,
120 const std::vector<pcl::PointIndices::Ptr> all_inliers,
121 pcl::PointIndices::Ptr& output_indices);
virtual void removeDuplicatedEdges(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< pcl::PointIndices::Ptr > inliers, const std::vector< pcl::ModelCoefficients::Ptr > coefficients, std::vector< pcl::PointIndices::Ptr > &output_inliers, std::vector< pcl::ModelCoefficients::Ptr > &output_coefficients)
double duplication_distance_threshold_
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
ros::Publisher pub_outlier_removed_coefficients_
virtual jsk_recognition_utils::Line::Ptr lineFromCoefficients(const pcl::ModelCoefficients::Ptr coefficients)
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > SyncPolicy
virtual boost::tuple< int, int > findMinMaxIndex(const int width, const int height, const std::vector< int > &indices)
virtual void removeOutliers(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< PCLIndicesMsg > &indices, std::vector< pcl::PointIndices::Ptr > &output_inliers, std::vector< pcl::ModelCoefficients::Ptr > &output_coefficients)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
ros::Publisher pub_coefficients_
virtual void publishIndices(ros::Publisher &pub, ros::Publisher &pub_coefficients, ros::Publisher &pub_edges, const std::vector< pcl::PointIndices::Ptr > inliers, const std::vector< pcl::ModelCoefficients::Ptr > coefficients, const std_msgs::Header &header)
double duplication_angle_threshold_
ros::Publisher pub_edges_
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void removeOutliersByLine(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< int > &indices, pcl::PointIndices &inliers, pcl::ModelCoefficients &coefficients)
jsk_pcl_ros::EdgeDepthRefinementConfig Config
ros::Publisher pub_outlier_removed_edges_
boost::mutex mutex
global mutex.
double outlier_distance_threshold_
virtual void refine(const sensor_msgs::PointCloud2ConstPtr &point, const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices)
virtual void integrateDuplicatedIndices(const pcl::PointCloud< PointT >::Ptr &cloud, const std::set< int > &duplicated_set, const std::vector< pcl::PointIndices::Ptr > all_inliers, pcl::PointIndices::Ptr &output_indices)
virtual void unsubscribe()
ros::Publisher pub_outlier_removed_indices_
virtual jsk_recognition_utils::Segment::Ptr segmentFromIndices(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< int > &indices, const jsk_recognition_utils::Line::Ptr &line)
ros::Publisher pub_indices_