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::updateDiagnostic(
00124 diagnostic_updater::DiagnosticStatusWrapper &stat)
00125 {
00126 if (vital_checker_->isAlive()) {
00127 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00128 "ColorHistogram running");
00129 }
00130 else {
00131 jsk_topic_tools::addDiagnosticErrorSummary(
00132 "ColorHistogram", vital_checker_, stat);
00133 }
00134 }
00135
00136 void ColorHistogram::convertHistogramToMsg(const cv::Mat& hist,
00137 int size,
00138 jsk_recognition_msgs::ColorHistogram& msg)
00139 {
00140 msg.histogram.clear();
00141 for (int i = 0; i < size; i++) {
00142 float val = hist.at<float>(0, i);
00143 msg.histogram.push_back(val);
00144 }
00145 }
00146
00147 void ColorHistogram::processBGR(const cv::Mat& bgr_image,
00148 const cv::Mat& mask,
00149 const std_msgs::Header& header)
00150 {
00151 float range[] = { 0, 256 } ;
00152 const float* histRange = { range };
00153 cv::MatND b_hist, g_hist, r_hist;
00154 bool uniform = true; bool accumulate = false;
00155 std::vector<cv::Mat> bgr_planes;
00156 split(bgr_image, bgr_planes);
00157
00158 cv::calcHist(&bgr_planes[0], 1, 0, mask, b_hist, 1, &b_hist_size_,
00159 &histRange, uniform, accumulate);
00160 cv::calcHist(&bgr_planes[1], 1, 0, mask, g_hist, 1, &g_hist_size_,
00161 &histRange, uniform, accumulate);
00162 cv::calcHist(&bgr_planes[2], 1, 0, mask, r_hist, 1, &r_hist_size_,
00163 &histRange, uniform, accumulate);
00164
00165 jsk_recognition_msgs::ColorHistogram b_histogram;
00166 b_histogram.header = header;
00167 convertHistogramToMsg(b_hist, b_hist_size_, b_histogram);
00168 b_hist_pub_.publish(b_histogram);
00169
00170 jsk_recognition_msgs::ColorHistogram g_histogram;
00171 g_histogram.header = header;
00172 convertHistogramToMsg(g_hist, g_hist_size_, g_histogram);
00173 g_hist_pub_.publish(g_histogram);
00174
00175 jsk_recognition_msgs::ColorHistogram r_histogram;
00176 r_histogram.header = header;
00177 convertHistogramToMsg(r_hist, r_hist_size_, r_histogram);
00178 r_hist_pub_.publish(r_histogram);
00179 }
00180
00181 void ColorHistogram::processBGR(const cv::Mat& bgr_image,
00182 const std_msgs::Header& header)
00183 {
00184 processBGR(bgr_image, cv::Mat(), header);
00185 }
00186
00187 void ColorHistogram::processHSI(const cv::Mat& bgr_image,
00188 const std_msgs::Header& header)
00189 {
00190 processHSI(bgr_image, cv::Mat(), header);
00191 }
00192
00193 void ColorHistogram::processHSI(const cv::Mat& bgr_image,
00194 const cv::Mat& mask,
00195 const std_msgs::Header& header)
00196 {
00197 cv::Mat hsi_image;
00198 cv::cvtColor(bgr_image, hsi_image, CV_BGR2HSV);
00199
00200 float range[] = { 0, 256 } ;
00201 const float* histRange = { range };
00202 float h_range[] = { 0, 180 } ;
00203 const float* h_histRange = { h_range };
00204 cv::MatND h_hist, s_hist, i_hist;
00205 bool uniform = true; bool accumulate = false;
00206 std::vector<cv::Mat> hsi_planes;
00207 split(hsi_image, hsi_planes);
00208
00209 cv::calcHist(&hsi_planes[0], 1, 0, mask, h_hist, 1, &h_hist_size_,
00210 &h_histRange, uniform, accumulate);
00211 cv::calcHist(&hsi_planes[1], 1, 0, mask, s_hist, 1, &s_hist_size_,
00212 &histRange, uniform, accumulate);
00213 cv::calcHist(&hsi_planes[2], 1, 0, mask, i_hist, 1, &i_hist_size_,
00214 &histRange, uniform, accumulate);
00215
00216 jsk_recognition_msgs::ColorHistogram h_histogram;
00217 h_histogram.header = header;
00218 convertHistogramToMsg(h_hist, h_hist_size_, h_histogram);
00219 h_hist_pub_.publish(h_histogram);
00220
00221 jsk_recognition_msgs::ColorHistogram s_histogram;
00222 s_histogram.header = header;
00223 convertHistogramToMsg(s_hist, s_hist_size_, s_histogram);
00224 s_hist_pub_.publish(s_histogram);
00225
00226 jsk_recognition_msgs::ColorHistogram i_histogram;
00227 i_histogram.header = header;
00228 convertHistogramToMsg(i_hist, i_hist_size_, i_histogram);
00229 i_hist_pub_.publish(i_histogram);
00230
00231 }
00232
00233 void ColorHistogram::extract(
00234 const sensor_msgs::Image::ConstPtr& image,
00235 const geometry_msgs::PolygonStamped::ConstPtr& rectangle)
00236 {
00237 vital_checker_->poke();
00238 boost::mutex::scoped_lock lock(mutex_);
00239 try
00240 {
00241 cv_bridge::CvImagePtr cv_ptr;
00242 cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00243 geometry_msgs::Point32 point0 = rectangle->polygon.points[0];
00244 geometry_msgs::Point32 point1 = rectangle->polygon.points[1];
00245 int min_x = std::max(0.0f, std::min(point0.x, point1.x));
00246 int min_y = std::max(0.0f, std::min(point0.y, point1.y));
00247 int max_x = std::min(std::max(point0.x, point1.x), (float)image->width);
00248 int max_y = std::min(std::max(point0.y, point1.y), (float)image->height);
00249 cv::Rect roi(min_x, min_y, max_x - min_x, max_y - min_y);
00250 cv::Mat bgr_image, roi_image;
00251 roi_image = cv_ptr->image(roi);
00252 if (image->encoding == sensor_msgs::image_encodings::RGB8) {
00253 cv::cvtColor(roi_image, bgr_image, CV_RGB2BGR);
00254 }
00255 else {
00256 roi_image.copyTo(bgr_image);
00257 }
00258 image_pub_.publish(cv_bridge::CvImage(
00259 image->header,
00260 sensor_msgs::image_encodings::BGR8,
00261 bgr_image).toImageMsg());
00262 processBGR(bgr_image, image->header);
00263 processHSI(bgr_image, image->header);
00264 }
00265 catch (cv_bridge::Exception& e)
00266 {
00267 NODELET_ERROR("cv_bridge exception: %s", e.what());
00268 return;
00269 }
00270 }
00271
00272 void ColorHistogram::extractMask(
00273 const sensor_msgs::Image::ConstPtr& image,
00274 const sensor_msgs::Image::ConstPtr& mask_image)
00275 {
00276 try {
00277 cv_bridge::CvImagePtr cv_ptr
00278 = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8);
00279 cv_bridge::CvImagePtr mask_ptr
00280 = cv_bridge::toCvCopy(mask_image, sensor_msgs::image_encodings::MONO8);
00281 cv::Mat bgr_image = cv_ptr->image;
00282 cv::Mat mask_image = mask_ptr->image;
00283 cv::Mat masked_image;
00284 bgr_image.copyTo(masked_image, mask_image);
00285 sensor_msgs::Image::Ptr ros_masked_image
00286 = cv_bridge::CvImage(image->header,
00287 sensor_msgs::image_encodings::BGR8,
00288 masked_image).toImageMsg();
00289 image_pub_.publish(ros_masked_image);
00290
00291 processBGR(bgr_image, mask_image, image->header);
00292 processHSI(bgr_image, mask_image, image->header);
00293
00294 }
00295 catch (cv_bridge::Exception& e)
00296 {
00297 NODELET_ERROR("cv_bridge exception: %s", e.what());
00298 return;
00299 }
00300
00301 }
00302 }
00303
00304 #include <pluginlib/class_list_macros.h>
00305 PLUGINLIB_EXPORT_CLASS(jsk_perception::ColorHistogram, nodelet::Nodelet);