Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "jsk_pcl_ros_utils/label_to_cluster_point_indices.h"
00036 #include "jsk_topic_tools/log_utils.h"
00037 #include <jsk_recognition_msgs/ClusterPointIndices.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <boost/assign.hpp>
00041 #include <map>
00042
00043 namespace jsk_pcl_ros_utils
00044 {
00045
00046 void LabelToClusterPointIndices::onInit()
00047 {
00048 DiagnosticNodelet::onInit();
00049 pnh_->param("bg_label", bg_label_, 0);
00050 pnh_->param("ignore_labels", ignore_labels_, std::vector<int>());
00051 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output", 1);
00052 pub_bg_ = advertise<pcl_msgs::PointIndices>(*pnh_, "output/bg_indices", 1);
00053 onInitPostProcess();
00054 }
00055
00056 void LabelToClusterPointIndices::subscribe()
00057 {
00058 sub_ = pnh_->subscribe("input", 1,
00059 &LabelToClusterPointIndices::convert,
00060 this);
00061 ros::V_string names = boost::assign::list_of("~input");
00062 jsk_topic_tools::warnNoRemap(names);
00063 }
00064
00065 void LabelToClusterPointIndices::unsubscribe()
00066 {
00067 sub_.shutdown();
00068 }
00069
00070 void LabelToClusterPointIndices::convert(
00071 const sensor_msgs::Image::ConstPtr& label_msg)
00072 {
00073 vital_checker_->poke();
00074 cv_bridge::CvImagePtr label_img_ptr = cv_bridge::toCvCopy(
00075 label_msg, sensor_msgs::image_encodings::TYPE_32SC1);
00076
00077 std::map<int, pcl_msgs::PointIndices> label_to_indices;
00078 int max_label = 0;
00079 for (size_t j = 0; j < label_img_ptr->image.rows; j++)
00080 {
00081 for (size_t i = 0; i < label_img_ptr->image.cols; i++)
00082 {
00083 int label = label_img_ptr->image.at<int>(j, i);
00084 if (label > max_label) {
00085 max_label = label;
00086 }
00087 label_to_indices[label].header = label_msg->header;
00088 label_to_indices[label].indices.push_back(j * label_img_ptr->image.cols + i);
00089 }
00090 }
00091
00092 jsk_recognition_msgs::ClusterPointIndices cluster_indices_msg;
00093 pcl_msgs::PointIndices bg_indices_msg;
00094 cluster_indices_msg.header = bg_indices_msg.header = label_msg->header;
00095 for (size_t i=0; i <= max_label; i++)
00096 {
00097 pcl_msgs::PointIndices indices_msg;
00098 if (i == bg_label_) {
00099 if (label_to_indices.count(i) == 0) {
00100 indices_msg.header = label_msg->header;
00101 bg_indices_msg = indices_msg;
00102 }
00103 else {
00104 bg_indices_msg = label_to_indices[i];
00105 }
00106 }
00107 else if (label_to_indices.count(i) == 0 ||
00108 std::find(ignore_labels_.begin(), ignore_labels_.end(), i) != ignore_labels_.end()) {
00109 indices_msg.header = label_msg->header;
00110 cluster_indices_msg.cluster_indices.push_back(indices_msg);
00111 }
00112 else {
00113 cluster_indices_msg.cluster_indices.push_back(label_to_indices[i]);
00114 }
00115 }
00116 pub_bg_.publish(bg_indices_msg);
00117 pub_.publish(cluster_indices_msg);
00118 }
00119
00120 }
00121
00122 #include <pluginlib/class_list_macros.h>
00123 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::LabelToClusterPointIndices, nodelet::Nodelet);