37 #include <jsk_recognition_msgs/ClusterPointIndices.h> 40 #include <boost/assign.hpp> 48 DiagnosticNodelet::onInit();
51 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*
pnh_,
"output", 1);
52 pub_bg_ = advertise<pcl_msgs::PointIndices>(*
pnh_,
"output/bg_indices", 1);
71 const sensor_msgs::Image::ConstPtr& label_msg)
77 std::map<int, pcl_msgs::PointIndices> label_to_indices;
79 for (
size_t j = 0; j < label_img_ptr->image.rows; j++)
81 for (
size_t i = 0; i < label_img_ptr->image.cols; i++)
83 int label = label_img_ptr->image.at<
int>(j, i);
84 if (label > max_label) {
87 label_to_indices[label].header = label_msg->header;
88 label_to_indices[label].indices.push_back(j * label_img_ptr->image.cols + i);
92 jsk_recognition_msgs::ClusterPointIndices cluster_indices_msg;
93 pcl_msgs::PointIndices bg_indices_msg;
94 cluster_indices_msg.header = bg_indices_msg.header = label_msg->header;
95 for (
size_t i=0; i <= max_label; i++)
97 pcl_msgs::PointIndices indices_msg;
99 if (label_to_indices.count(i) == 0) {
100 indices_msg.header = label_msg->header;
101 bg_indices_msg = indices_msg;
104 bg_indices_msg = label_to_indices[i];
107 else if (label_to_indices.count(i) == 0 ||
109 indices_msg.header = label_msg->header;
110 cluster_indices_msg.cluster_indices.push_back(indices_msg);
113 cluster_indices_msg.cluster_indices.push_back(label_to_indices[i]);
void publish(const boost::shared_ptr< M > &message) const
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::LabelToClusterPointIndices, nodelet::Nodelet)
std::vector< int > ignore_labels_
std::vector< std::string > V_string
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void unsubscribe()
virtual void convert(const sensor_msgs::Image::ConstPtr &label_msg)
const std::string TYPE_32SC1