36 #ifndef JSK_PCL_ROS_CLUSTER_POINT_INDICES_DECOMPOSER_H_ 37 #define JSK_PCL_ROS_CLUSTER_POINT_INDICES_DECOMPOSER_H_ 42 #include "jsk_recognition_msgs/ClusterPointIndices.h" 43 #include "jsk_recognition_msgs/PolygonArray.h" 44 #include "jsk_recognition_msgs/ModelCoefficientsArray.h" 47 #include "sensor_msgs/PointCloud2.h" 48 #include <dynamic_reconfigure/server.h> 53 #include <pcl/point_types.h> 54 #include <pcl/impl/point_types.hpp> 56 #include <std_msgs/ColorRGBA.h> 57 #include <jsk_recognition_msgs/BoundingBoxArray.h> 64 #include "jsk_pcl_ros/ClusterPointIndicesDecomposerConfig.h" 72 typedef jsk_pcl_ros::ClusterPointIndicesDecomposerConfig
Config;
74 sensor_msgs::PointCloud2,
77 sensor_msgs::PointCloud2,
80 sensor_msgs::PointCloud2,
81 jsk_recognition_msgs::ClusterPointIndices,
82 jsk_recognition_msgs::PolygonArray,
85 sensor_msgs::PointCloud2,
86 jsk_recognition_msgs::ClusterPointIndices,
87 jsk_recognition_msgs::PolygonArray,
90 virtual void extract(
const sensor_msgs::PointCloud2ConstPtr &
point,
91 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices,
92 const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
93 const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients);
94 virtual void extract(
const sensor_msgs::PointCloud2ConstPtr &
point,
95 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices);
96 virtual void sortIndicesOrder(
const pcl::PointCloud<pcl::PointXYZ>::Ptr input,
97 const std::vector<pcl::IndicesPtr> indices_array,
98 std::vector<size_t>* argsort);
100 const std::vector<pcl::IndicesPtr> indices_array,
101 std::vector<size_t>* argsort);
103 const std::vector<pcl::IndicesPtr> indices_array,
104 std::vector<size_t>* argsort);
106 const std::vector<pcl::IndicesPtr> indices_array,
107 std::vector<size_t>* argsort);
113 (
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
115 pcl::PointCloud<pcl::PointXYZRGB>& debug_output);
118 (
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
119 const std_msgs::Header
header,
120 const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
121 const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
122 geometry_msgs::Pose& center_pose_msg,
123 jsk_recognition_msgs::BoundingBox& bounding_box);
126 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
127 pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud_transformed,
128 const Eigen::Vector4f center,
129 const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
130 const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
132 Eigen::Quaternionf& q,
133 int& nearest_plane_index);
136 const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
137 const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients);
144 const sensor_msgs::PointCloud2ConstPtr &input,
145 const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices_input);
152 r = (uint8_t)(c.r * 255);
153 g = (uint8_t)(c.g * 255);
154 b = (uint8_t)(c.b * 255);
155 return ((uint32_t)r<<16 | (uint32_t)g<<8 | (uint32_t)b);
198 #endif // JSK_PCL_ROS_CLUSTER_POINT_INDICES_DECOMPOSER_H_
bool force_to_flip_z_axis_
ros::Publisher centers_pub_
static uint32_t colorRGBAToUInt32(std_msgs::ColorRGBA c)
virtual bool computeCenterAndBoundingBox(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr segmented_cloud, const std_msgs::Header header, const jsk_recognition_msgs::PolygonArrayConstPtr &planes, const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr &coefficients, geometry_msgs::Pose ¢er_pose_msg, jsk_recognition_msgs::BoundingBox &bounding_box)
virtual void configCallback(Config &config, uint32_t level)
ros::Publisher label_pub_
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices, jsk_recognition_msgs::PolygonArray, jsk_recognition_msgs::ModelCoefficientsArray > SyncAlignPolicy
void sortIndicesOrderByZAxis(const pcl::PointCloud< pcl::PointXYZ >::Ptr input, const std::vector< pcl::IndicesPtr > indices_array, std::vector< size_t > *argsort)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > SyncPolicy
bool fill_boxes_label_with_nearest_plane_index_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncAlignPolicy > > async_align_
virtual bool transformPointCloudToAlignWithPlane(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr segmented_cloud, pcl::PointCloud< pcl::PointXYZRGB >::Ptr segmented_cloud_transformed, const Eigen::Vector4f center, const jsk_recognition_msgs::PolygonArrayConstPtr &planes, const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr &coefficients, Eigen::Matrix4f &m4, Eigen::Quaternionf &q, int &nearest_plane_index)
virtual void unsubscribe()
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices, jsk_recognition_msgs::PolygonArray, jsk_recognition_msgs::ModelCoefficientsArray > ApproximateSyncAlignPolicy
jsk_recognition_utils::Counter cluster_counter_
ros::Publisher indices_pub_
virtual void allocatePublishers(size_t num)
void sortIndicesOrderByCloudSize(const pcl::PointCloud< pcl::PointXYZ >::Ptr input, const std::vector< pcl::IndicesPtr > indices_array, std::vector< size_t > *argsort)
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_
tf::TransformListener * tf_listener_
std::vector< ros::Publisher > publishers_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
boost::shared_ptr< tf::TransformBroadcaster > br_
virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_target_
virtual void publishNegativeIndices(const sensor_msgs::PointCloud2ConstPtr &input, const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices_input)
jsk_pcl_ros::ClusterPointIndicesDecomposerConfig Config
virtual int findNearestPlane(const Eigen::Vector4f ¢er, const jsk_recognition_msgs::PolygonArrayConstPtr &planes, const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr &coefficients)
void sortIndicesOrderByIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr input, const std::vector< pcl::IndicesPtr > indices_array, std::vector< size_t > *argsort)
boost::mutex mutex
global mutex.
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
std::string target_frame_id_
void addToDebugPointCloud(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr segmented_cloud, size_t i, pcl::PointCloud< pcl::PointXYZRGB > &debug_output)
virtual void sortIndicesOrder(const pcl::PointCloud< pcl::PointXYZ >::Ptr input, const std::vector< pcl::IndicesPtr > indices_array, std::vector< size_t > *argsort)
ros::Publisher negative_indices_pub_
ClusterPointIndicesDecomposer()
virtual void extract(const sensor_msgs::PointCloud2ConstPtr &point, const jsk_recognition_msgs::ClusterPointIndicesConstPtr &indices, const jsk_recognition_msgs::PolygonArrayConstPtr &planes, const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr &coefficients)
boost::shared_ptr< message_filters::Synchronizer< SyncAlignPolicy > > sync_align_
message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::ClusterPointIndices > ApproximateSyncPolicy
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
bool align_boxes_with_plane_