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/color_histogram.h"
00037 #include <jsk_topic_tools/log_utils.h>
00038
00039 namespace jsk_perception
00040 {
00041 void ColorHistogram::configCallback(Config &new_config, uint32_t level)
00042 {
00043 boost::mutex::scoped_lock lock(mutex_);
00044 b_hist_size_ = new_config.blue_histogram_bin;
00045 g_hist_size_ = new_config.green_histogram_bin;
00046 r_hist_size_ = new_config.red_histogram_bin;
00047 h_hist_size_ = new_config.hue_histogram_bin;
00048 s_hist_size_ = new_config.saturation_histogram_bin;
00049 i_hist_size_ = new_config.intensity_histogram_bin;
00050 onInitPostProcess();
00051 }
00052
00053 void ColorHistogram::onInit()
00054 {
00055 DiagnosticNodelet::onInit();
00056 nh_ = ros::NodeHandle(getNodeHandle(), "image");
00057 pnh_->param("use_mask", use_mask_, false);
00058 b_hist_size_ = r_hist_size_ = g_hist_size_ =
00059 h_hist_size_ = s_hist_size_ = i_hist_size_ = 512;
00060 b_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00061 *pnh_, "blue_histogram", 1);
00062 g_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00063 *pnh_, "green_histogram", 1);
00064 r_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00065 *pnh_, "red_histogram", 1);
00066 h_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00067 *pnh_, "hue_histogram", 1);
00068 s_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00069 *pnh_, "saturation_histogram", 1);
00070 i_hist_pub_ = advertise<jsk_recognition_msgs::ColorHistogram>(
00071 *pnh_, "intensity_histogram", 1);
00072 image_pub_ = advertise<sensor_msgs::Image>(
00073 *pnh_, "input_image", 1);
00074
00075 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00076 dynamic_reconfigure::Server<Config>::CallbackType f =
00077 boost::bind (&ColorHistogram::configCallback, this, _1, _2);
00078 srv_->setCallback (f);
00079 }
00080
00081 void ColorHistogram::subscribe()
00082 {
00083 ros::V_string names;
00084 if (!use_mask_) {
00085 it_.reset(new image_transport::ImageTransport(nh_));
00086 image_sub_.subscribe(*it_, "", 1);
00087 rectangle_sub_.subscribe(nh_, "screenrectangle", 1);
00088 names.push_back("screenrectangle");
00089 sync_
00090 = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
00091 sync_->connectInput(image_sub_, rectangle_sub_);
00092 sync_->registerCallback(boost::bind(
00093 &ColorHistogram::extract, this, _1, _2));
00094 }
00095 else {
00096 it_.reset(new image_transport::ImageTransport(nh_));
00097 image_sub_.subscribe(*it_, "", 1);
00098 image_mask_sub_.subscribe(*it_, "mask", 1);
00099 names.push_back("mask");
00100 mask_sync_
00101 = boost::make_shared<message_filters::Synchronizer<
00102 MaskSyncPolicy> >(100);
00103 mask_sync_->connectInput(image_sub_, image_mask_sub_);
00104 mask_sync_->registerCallback(
00105 boost::bind(
00106 &ColorHistogram::extractMask, this, _1, _2));
00107 }
00108 jsk_topic_tools::warnNoRemap(names);
00109 }
00110
00111 void ColorHistogram::unsubscribe()
00112 {
00113 if (!use_mask_) {
00114 image_sub_.unsubscribe();
00115 rectangle_sub_.unsubscribe();
00116 }
00117 else {
00118 image_sub_.unsubscribe();
00119 image_mask_sub_.unsubscribe();
00120 }
00121 }
00122
00123 void ColorHistogram::convertHistogramToMsg(const cv::Mat& hist,
00124 int size,
00125 jsk_recognition_msgs::ColorHistogram& msg)
00126 {
00127 msg.histogram.clear();
00128 for (int i = 0; i < size; i++) {
00129 float val = hist.at<float>(0, i);
00130 msg.histogram.push_back(val);
00131 }
00132 }
00133
00134 void ColorHistogram::processBGR(const cv::Mat& bgr_image,
00135 const cv::Mat& mask,
00136 const std_msgs::Header& header)
00137 {
00138 float range[] = { 0, 256 } ;
00139 const float* histRange = { range };
00140 cv::MatND b_hist, g_hist, r_hist;
00141 bool uniform = true; bool accumulate = false;
00142 std::vector<cv::Mat> bgr_planes;
00143 split(bgr_image, bgr_planes);
00144
00145 cv::calcHist(&bgr_planes[0], 1, 0, mask, b_hist, 1, &b_hist_size_,
00146 &histRange, uniform, accumulate);
00147 cv::calcHist(&bgr_planes[1], 1, 0, mask, g_hist, 1, &g_hist_size_,
00148 &histRange, uniform, accumulate);
00149 cv::calcHist(&bgr_planes[2], 1, 0, mask, r_hist, 1, &r_hist_size_,
00150 &histRange, uniform, accumulate);
00151
00152 jsk_recognition_msgs::ColorHistogram b_histogram;
00153 b_histogram.header = header;
00154 convertHistogramToMsg(b_hist, b_hist_size_, b_histogram);
00155 b_hist_pub_.publish(b_histogram);
00156
00157 jsk_recognition_msgs::ColorHistogram g_histogram;
00158 g_histogram.header = header;
00159 convertHistogramToMsg(g_hist, g_hist_size_, g_histogram);
00160 g_hist_pub_.publish(g_histogram);
00161
00162 jsk_recognition_msgs::ColorHistogram r_histogram;
00163 r_histogram.header = header;
00164 convertHistogramToMsg(r_hist, r_hist_size_, r_histogram);
00165 r_hist_pub_.publish(r_histogram);
00166 }
00167
00168 void ColorHistogram::processBGR(const cv::Mat& bgr_image,
00169 const std_msgs::Header& header)
00170 {
00171 processBGR(bgr_image, cv::Mat(), header);
00172 }
00173
00174 void ColorHistogram::processHSI(const cv::Mat& bgr_image,
00175 const std_msgs::Header& header)
00176 {
00177 processHSI(bgr_image, cv::Mat(), header);
00178 }
00179
00180 void ColorHistogram::processHSI(const cv::Mat& bgr_image,
00181 const cv::Mat& mask,
00182 const std_msgs::Header& header)
00183 {
00184 cv::Mat hsi_image;
00185 cv::cvtColor(bgr_image, hsi_image, CV_BGR2HSV);
00186
00187 float range[] = { 0, 256 } ;
00188 const float* histRange = { range };
00189 float h_range[] = { 0, 180 } ;
00190 const float* h_histRange = { h_range };
00191 cv::MatND h_hist, s_hist, i_hist;
00192 bool uniform = true; bool accumulate = false;
00193 std::vector<cv::Mat> hsi_planes;
00194 split(hsi_image, hsi_planes);
00195
00196 cv::calcHist(&hsi_planes[0], 1, 0, mask, h_hist, 1, &h_hist_size_,
00197 &h_histRange, uniform, accumulate);
00198 cv::calcHist(&hsi_planes[1], 1, 0, mask, s_hist, 1, &s_hist_size_,
00199 &histRange, uniform, accumulate);
00200 cv::calcHist(&hsi_planes[2], 1, 0, mask, i_hist, 1, &i_hist_size_,
00201 &histRange, uniform, accumulate);
00202
00203 jsk_recognition_msgs::ColorHistogram h_histogram;
00204 h_histogram.header = header;
00205 convertHistogramToMsg(h_hist, h_hist_size_, h_histogram);
00206 h_hist_pub_.publish(h_histogram);
00207
00208 jsk_recognition_msgs::ColorHistogram s_histogram;
00209 s_histogram.header = header;
00210 convertHistogramToMsg(s_hist, s_hist_size_, s_histogram);
00211 s_hist_pub_.publish(s_histogram);
00212
00213 jsk_recognition_msgs::ColorHistogram i_histogram;
00214 i_histogram.header = header;
00215 convertHistogramToMsg(i_hist, i_hist_size_, i_histogram);
00216 i_hist_pub_.publish(i_histogram);
00217
00218 }
00219
00220 void ColorHistogram::extract(
00221 const sensor_msgs::Image::ConstPtr& image,
00222 const geometry_msgs::PolygonStamped::ConstPtr& rectangle)
00223 {
00224 vital_checker_->poke();
00225 boost::mutex::scoped_lock lock(mutex_);
00226 try
00227 {
00228 cv_bridge::CvImagePtr cv_ptr;
00229 cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00230 geometry_msgs::Point32 point0 = rectangle->polygon.points[0];
00231 geometry_msgs::Point32 point1 = rectangle->polygon.points[1];
00232 int min_x = std::max(0.0f, std::min(point0.x, point1.x));
00233 int min_y = std::max(0.0f, std::min(point0.y, point1.y));
00234 int max_x = std::min(std::max(point0.x, point1.x), (float)image->width);
00235 int max_y = std::min(std::max(point0.y, point1.y), (float)image->height);
00236 cv::Rect roi(min_x, min_y, max_x - min_x, max_y - min_y);
00237 cv::Mat bgr_image, roi_image;
00238 roi_image = cv_ptr->image(roi);
00239 if (image->encoding == sensor_msgs::image_encodings::RGB8) {
00240 cv::cvtColor(roi_image, bgr_image, CV_RGB2BGR);
00241 }
00242 else {
00243 roi_image.copyTo(bgr_image);
00244 }
00245 image_pub_.publish(cv_bridge::CvImage(
00246 image->header,
00247 sensor_msgs::image_encodings::BGR8,
00248 bgr_image).toImageMsg());
00249 processBGR(bgr_image, image->header);
00250 processHSI(bgr_image, image->header);
00251 }
00252 catch (cv_bridge::Exception& e)
00253 {
00254 NODELET_ERROR("cv_bridge exception: %s", e.what());
00255 return;
00256 }
00257 }
00258
00259 void ColorHistogram::extractMask(
00260 const sensor_msgs::Image::ConstPtr& image,
00261 const sensor_msgs::Image::ConstPtr& mask_image)
00262 {
00263 try {
00264 cv_bridge::CvImagePtr cv_ptr
00265 = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00266 cv_bridge::CvImagePtr mask_ptr
00267 = cv_bridge::toCvCopy(mask_image, sensor_msgs::image_encodings::MONO8);
00268 cv::Mat bgr_image = cv_ptr->image;
00269 cv::Mat mask_image = mask_ptr->image;
00270 cv::Mat masked_image;
00271 bgr_image.copyTo(masked_image, mask_image);
00272 sensor_msgs::Image::Ptr ros_masked_image
00273 = cv_bridge::CvImage(image->header,
00274 sensor_msgs::image_encodings::BGR8,
00275 masked_image).toImageMsg();
00276 image_pub_.publish(ros_masked_image);
00277
00278 processBGR(bgr_image, mask_image, image->header);
00279 processHSI(bgr_image, mask_image, image->header);
00280
00281 }
00282 catch (cv_bridge::Exception& e)
00283 {
00284 NODELET_ERROR("cv_bridge exception: %s", e.what());
00285 return;
00286 }
00287
00288 }
00289 }
00290
00291 #include <pluginlib/class_list_macros.h>
00292 PLUGINLIB_EXPORT_CLASS(jsk_perception::ColorHistogram, nodelet::Nodelet);