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
00036 #include "jsk_perception/colorize_labels.h"
00037 #include <jsk_topic_tools/color_utils.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <opencv2/opencv.hpp>
00040 #include <sensor_msgs/image_encodings.h>
00041
00042 namespace jsk_perception
00043 {
00044 void ColorizeLabels::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 pub_ = advertise<sensor_msgs::Image>(
00048 *pnh_, "output", 1);
00049 }
00050
00051 void ColorizeLabels::subscribe()
00052 {
00053 sub_ = pnh_->subscribe("input", 1, &ColorizeLabels::colorize, this);
00054 }
00055
00056 void ColorizeLabels::unsubscribe()
00057 {
00058 sub_.shutdown();
00059 }
00060
00061 void ColorizeLabels::colorize(
00062 const sensor_msgs::Image::ConstPtr& label_image_msg)
00063 {
00064 cv::Mat label_image = cv_bridge::toCvShare(
00065 label_image_msg, label_image_msg->encoding)->image;
00066 cv::Mat output_image = cv::Mat::zeros(label_image_msg->height,
00067 label_image_msg->width,
00068 CV_8UC3);
00069 JSK_ROS_INFO("%dx%d", label_image_msg->width, label_image_msg->height);
00070 for (size_t j = 0; j < label_image.rows; ++j) {
00071 for (size_t i = 0; i < label_image.cols; ++i) {
00072 int label = label_image.at<int>(j, i);
00073 if (label == 0) {
00074 output_image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
00075 }
00076 else {
00077 std_msgs::ColorRGBA rgba = jsk_topic_tools::colorCategory20(label);
00078 output_image.at<cv::Vec3b>(j, i)
00079 = cv::Vec3b(int(rgba.b * 255), int(rgba.g * 255), int(rgba.r * 255));
00080 }
00081 }
00082 }
00083 pub_.publish(
00084 cv_bridge::CvImage(
00085 label_image_msg->header,
00086 sensor_msgs::image_encodings::BGR8,
00087 output_image).toImageMsg());
00088 }
00089 }
00090
00091 #include <pluginlib/class_list_macros.h>
00092 PLUGINLIB_EXPORT_CLASS (jsk_perception::ColorizeLabels, nodelet::Nodelet);