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