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_float_image.h"
00037 #include <sensor_msgs/image_encodings.h>
00038
00039 namespace jsk_perception
00040 {
00041 void ColorizeFloatImage::onInit()
00042 {
00043 DiagnosticNodelet::onInit();
00044 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00045 }
00046
00047 void ColorizeFloatImage::subscribe()
00048 {
00049 sub_ = pnh_->subscribe("input", 1, &ColorizeFloatImage::colorize, this);
00050 }
00051
00052 void ColorizeFloatImage::unsubscribe()
00053 {
00054 sub_.shutdown();
00055 }
00056
00057 void ColorizeFloatImage::colorize(const sensor_msgs::Image::ConstPtr& msg)
00058 {
00059 cv::Mat float_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_32FC1)->image;
00060 cv::Mat color_image = cv::Mat(float_image.rows, float_image.cols, CV_8UC3);
00061 float min_value = FLT_MAX;
00062 float max_value = - FLT_MAX;
00063 for (size_t j = 0; j < float_image.rows; j++) {
00064 for (size_t i = 0; i < float_image.cols; i++) {
00065 float v = float_image.at<float>(j, i);
00066 if (v != - FLT_MAX) {
00067 min_value = std::min(v, min_value);
00068 max_value = std::max(v, max_value);
00069 }
00070 }
00071 }
00072 for (size_t j = 0; j < float_image.rows; j++) {
00073 for (size_t i = 0; i < float_image.cols; i++) {
00074 float v = float_image.at<float>(j, i);
00075 if (v != - FLT_MAX) {
00076 std_msgs::ColorRGBA c = jsk_topic_tools::heatColor((v - min_value) / (max_value - min_value));
00077 color_image.at<cv::Vec3b>(j, i) = cv::Vec3b(c.r * 255, c.g * 255, c.b * 255);
00078 }
00079 else {
00080 color_image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
00081 }
00082 }
00083 }
00084 pub_.publish(cv_bridge::CvImage(msg->header,
00085 sensor_msgs::image_encodings::RGB8,
00086 color_image).toImageMsg());
00087 }
00088
00089 }
00090
00091 #include <pluginlib/class_list_macros.h>
00092 PLUGINLIB_EXPORT_CLASS (jsk_perception::ColorizeFloatImage, nodelet::Nodelet);