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/overlay_image_color_on_mono.h"
00037 #include <boost/assign.hpp>
00038 #include <nodelet/nodelet.h>
00039 #include <opencv2/opencv.hpp>
00040 #include <cv_bridge/cv_bridge.h>
00041 #include <jsk_topic_tools/log_utils.h>
00042 #include <jsk_recognition_utils/cv_utils.h>
00043 #include <sensor_msgs/image_encodings.h>
00044
00045 namespace jsk_perception
00046 {
00047 void OverlayImageColorOnMono::onInit()
00048 {
00049 DiagnosticNodelet::onInit();
00050 pnh_->param("approximate_sync", approximate_sync_, false);
00051 pnh_->param("queue_size", queue_size_, 100);
00052 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00053 dynamic_reconfigure::Server<Config>::CallbackType f =
00054 boost::bind (
00055 &OverlayImageColorOnMono::configCallback, this, _1, _2);
00056 srv_->setCallback (f);
00057 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00058 onInitPostProcess();
00059 }
00060
00061 void OverlayImageColorOnMono::configCallback(Config &config, uint32_t level)
00062 {
00063 boost::mutex::scoped_lock lock(mutex_);
00064 color_alpha_ = config.color_alpha;
00065 }
00066
00067 void OverlayImageColorOnMono::subscribe()
00068 {
00069 sub_color_.subscribe(*pnh_, "input/color", 1);
00070 sub_mono_.subscribe(*pnh_, "input/mono", 1);
00071 if (approximate_sync_) {
00072 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(queue_size_);
00073 async_->connectInput(sub_color_, sub_mono_);
00074 async_->registerCallback(boost::bind(&OverlayImageColorOnMono::overlay, this, _1, _2));
00075 } else {
00076 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00077 sync_->connectInput(sub_color_, sub_mono_);
00078 sync_->registerCallback(boost::bind(&OverlayImageColorOnMono::overlay, this, _1, _2));
00079 }
00080 ros::V_string names = boost::assign::list_of("~input/color")("~input/mono");
00081 jsk_topic_tools::warnNoRemap(names);
00082 }
00083
00084 void OverlayImageColorOnMono::unsubscribe()
00085 {
00086 sub_color_.unsubscribe();
00087 sub_mono_.unsubscribe();
00088 }
00089
00090 void OverlayImageColorOnMono::overlay(const sensor_msgs::Image::ConstPtr& color_imgmsg,
00091 const sensor_msgs::Image::ConstPtr& mono_imgmsg)
00092 {
00093
00094 if (sensor_msgs::image_encodings::numChannels(color_imgmsg->encoding) != 3) {
00095 NODELET_ERROR_THROTTLE(10, "Input ~image/color message must be 3 channels color image. (RGB/BGR).");
00096 return;
00097 }
00098
00099 if (! ((color_imgmsg->height == mono_imgmsg->height) && (color_imgmsg->width == mono_imgmsg->width))) {
00100 NODELET_ERROR_THROTTLE(
00101 10, "The size of input color and mono image is different: (color: h=%d w=%d), (mono: h=%d w=%d)",
00102 color_imgmsg->height, color_imgmsg->width, mono_imgmsg->height, mono_imgmsg->width);
00103 return;
00104 }
00105
00106 boost::mutex::scoped_lock lock(mutex_);
00107
00108 cv::Mat color_image = cv_bridge::toCvShare(color_imgmsg, color_imgmsg->encoding)->image;
00109 cv::Mat mono_image = cv_bridge::toCvShare(mono_imgmsg, sensor_msgs::image_encodings::MONO8)->image;
00110
00111 cv::Mat overlayed_image = cv::Mat::zeros(color_image.rows, color_image.cols, CV_8UC3);
00112 for (size_t j = 0; j < overlayed_image.rows; j++) {
00113 for (size_t i = 0; i < overlayed_image.cols; i++) {
00114 cv::Vec3b color = color_image.at<cv::Vec3b>(j, i);
00115 uchar mono = mono_image.at<uchar>(j, i);
00116 overlayed_image.at<cv::Vec3b>(j, i) = cv::Vec3b(color[0] * color_alpha_ + mono * (1 - color_alpha_),
00117 color[1] * color_alpha_ + mono * (1 - color_alpha_),
00118 color[2] * color_alpha_ + mono * (1 - color_alpha_));
00119 }
00120 }
00121 pub_.publish(cv_bridge::CvImage(color_imgmsg->header,
00122 color_imgmsg->encoding,
00123 overlayed_image).toImageMsg());
00124 }
00125
00126 }
00127
00128 #include <pluginlib/class_list_macros.h>
00129 PLUGINLIB_EXPORT_CLASS (jsk_perception::OverlayImageColorOnMono, nodelet::Nodelet);