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_label_match.h"
00037 #include <boost/assign.hpp>
00038 #include <jsk_topic_tools/log_utils.h>
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <jsk_topic_tools/color_utils.h>
00041 #include <sensor_msgs/image_encodings.h>
00042
00043 namespace jsk_perception
00044 {
00045 void ColorHistogramLabelMatch::onInit()
00046 {
00047 DiagnosticNodelet::onInit();
00048 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049 dynamic_reconfigure::Server<Config>::CallbackType f =
00050 boost::bind (
00051 &ColorHistogramLabelMatch::configCallback, this, _1, _2);
00052 srv_->setCallback (f);
00053 pnh_->param("use_mask", use_mask_, false);
00054 pub_debug_ = advertise<sensor_msgs::Image>(
00055 *pnh_, "debug", 1);
00056 pub_coefficient_image_ = advertise<sensor_msgs::Image>(
00057 *pnh_, "output/coefficient_image", 1);
00058 pub_result_ = advertise<sensor_msgs::Image>(
00059 *pnh_, "output/extracted_region", 1);
00060 onInitPostProcess();
00061 }
00062
00063 void ColorHistogramLabelMatch::subscribe()
00064 {
00065 sub_image_.subscribe(*pnh_, "input", 1);
00066 sub_label_.subscribe(*pnh_, "input/label", 1);
00067 ros::V_string names = boost::assign::list_of("~input")("~input/label");
00068 if (use_mask_) {
00069 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00070 sub_mask_.subscribe(*pnh_, "input/mask", 1);
00071 names.push_back("~input/mask");
00072 sync_->connectInput(sub_image_, sub_label_, sub_mask_);
00073 sync_->registerCallback(
00074 boost::bind(
00075 &ColorHistogramLabelMatch::match, this, _1, _2, _3));
00076 }
00077 else {
00078 sync_wo_mask_ = boost::make_shared<message_filters::Synchronizer<SyncPolicyWithoutMask> >(100);
00079 sync_wo_mask_->connectInput(sub_image_, sub_label_);
00080 sync_wo_mask_->registerCallback(
00081 boost::bind(
00082 &ColorHistogramLabelMatch::match, this, _1, _2));
00083 }
00084 sub_histogram_ = pnh_->subscribe(
00085 "input/histogram", 1, &ColorHistogramLabelMatch::histogramCallback, this);
00086 names.push_back("~input/histogram");
00087 jsk_topic_tools::warnNoRemap(names);
00088 }
00089
00090 void ColorHistogramLabelMatch::unsubscribe()
00091 {
00092 sub_image_.unsubscribe();
00093 sub_label_.unsubscribe();
00094 if (use_mask_) {
00095 sub_mask_.unsubscribe();
00096 }
00097 sub_histogram_.shutdown();
00098 }
00099
00100 void ColorHistogramLabelMatch::getLabels(const cv::Mat& label,
00101 std::vector<int>& keys)
00102 {
00103 std::map<int, bool> map;
00104 for (int j = 0; j < label.rows; j++) {
00105 for (int i = 0; i < label.cols; i++) {
00106 int label_value = label.at<int>(j, i);
00107 if (map.find(label_value) == map.end()) {
00108 map[label_value] = true;
00109 }
00110 }
00111 }
00112
00113 for (std::map<int, bool>::iterator it = map.begin();
00114 it != map.end();
00115 ++it) {
00116 keys.push_back(it->first);
00117 }
00118 }
00119
00120 void ColorHistogramLabelMatch::getMaskImage(const cv::Mat& label_image,
00121 const int label,
00122 cv::Mat& mask)
00123 {
00124 for (int j = 0; j < label_image.rows; j++) {
00125 for (int i = 0; i < label_image.cols; i++) {
00126 if (label_image.at<int>(j, i) == label) {
00127 mask.at<uchar>(j, i) = 255;
00128 }
00129 }
00130 }
00131 }
00132
00133 bool ColorHistogramLabelMatch::isMasked(
00134 const cv::Mat& original_image,
00135 const cv::Mat& masked_image)
00136 {
00137 int original_count = 0;
00138 int masked_count = 0;
00139 for (int j = 0; j < original_image.rows; j++) {
00140 for (int i = 0; i < original_image.cols; i++) {
00141 if (original_image.at<uchar>(j, i) != 0) {
00142 original_count++;
00143 }
00144 if (masked_image.at<uchar>(j, i) != 0) {
00145 masked_count++;
00146 }
00147 }
00148 }
00149 return original_count != masked_count;
00150 }
00151
00152 void ColorHistogramLabelMatch::match(
00153 const sensor_msgs::Image::ConstPtr& image_msg,
00154 const sensor_msgs::Image::ConstPtr& label_msg)
00155 {
00156 cv::Mat whole_mask = cv::Mat(image_msg->height,
00157 image_msg->width, CV_8UC1, cv::Scalar(255));
00158 sensor_msgs::Image::ConstPtr mask_msg
00159 = cv_bridge::CvImage(image_msg->header,
00160 sensor_msgs::image_encodings::MONO8,
00161 whole_mask).toImageMsg();
00162 match(image_msg, label_msg, mask_msg);
00163 }
00164
00165
00166 void ColorHistogramLabelMatch::match(
00167 const sensor_msgs::Image::ConstPtr& image_msg,
00168 const sensor_msgs::Image::ConstPtr& label_msg,
00169 const sensor_msgs::Image::ConstPtr& mask_msg)
00170 {
00171 boost::mutex::scoped_lock lock(mutex_);
00172 if (histogram_.empty()) {
00173 NODELET_DEBUG("no reference histogram is available");
00174 return;
00175 }
00176
00177 cv::Mat image
00178 = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
00179 cv::Mat label
00180 = cv_bridge::toCvShare(label_msg, label_msg->encoding)->image;
00181 cv::Mat whole_mask
00182 = cv_bridge::toCvShare(mask_msg, mask_msg->encoding)->image;
00183
00184 cv::Mat coefficient_image = cv::Mat::zeros(
00185 image_msg->height, image_msg->width, CV_32FC1);
00186 std::vector<int> labels;
00187 getLabels(label, labels);
00188
00189 cv::Mat coefficients_heat_image = cv::Mat::zeros(
00190 image_msg->height, image_msg->width, CV_8UC3);
00191 int hist_size = histogram_.cols;
00192 float range[] = { min_value_, max_value_ };
00193 const float* hist_range = { range };
00194 double min_coef = DBL_MAX;
00195 double max_coef = - DBL_MAX;
00196 for (size_t i = 0; i < labels.size(); i++) {
00197 int label_index = labels[i];
00198 cv::Mat label_mask = cv::Mat::zeros(label.rows, label.cols, CV_8UC1);
00199 getMaskImage(label, label_index, label_mask);
00200 double coef = 0.0;
00201
00202 cv::Mat masked_label;
00203 label_mask.copyTo(masked_label, whole_mask);
00204 if (isMasked(label_mask, masked_label)) {
00205 coef = masked_coefficient_;
00206 }
00207 else {
00208 cv::MatND hist;
00209 bool uniform = true; bool accumulate = false;
00210 cv::calcHist(&image, 1, 0, label_mask, hist, 1,
00211 &hist_size, &hist_range, uniform, accumulate);
00212 cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2, -1,
00213 cv::Mat());
00214 cv::Mat hist_mat = cv::Mat::zeros(1, hist_size, CV_32FC1);
00215 for (size_t j = 0; j < hist_size; j++) {
00216 hist_mat.at<float>(0, j) = hist.at<float>(0, j);
00217 }
00218
00219
00220 coef = coefficients(hist_mat, histogram_);
00221 if (min_coef > coef) {
00222 min_coef = coef;
00223 }
00224 if (max_coef < coef) {
00225 max_coef = coef;
00226 }
00227 }
00228 std_msgs::ColorRGBA coef_color = jsk_topic_tools::heatColor(coef);
00229 for (size_t j = 0; j < coefficients_heat_image.rows; j++) {
00230 for (size_t i = 0; i < coefficients_heat_image.cols; i++) {
00231 if (label_mask.at<uchar>(j, i) == 255) {
00232 coefficients_heat_image.at<cv::Vec3b>(j, i)
00233 = cv::Vec3b(int(coef_color.b * 255),
00234 int(coef_color.g * 255),
00235 int(coef_color.r * 255));
00236 coefficient_image.at<float>(j, i) = coef;
00237 }
00238 }
00239 }
00240 }
00241 NODELET_INFO("coef: %f - %f", min_coef, max_coef);
00242 pub_debug_.publish(
00243 cv_bridge::CvImage(image_msg->header,
00244 sensor_msgs::image_encodings::BGR8,
00245 coefficients_heat_image).toImageMsg());
00246 pub_coefficient_image_.publish(
00247 cv_bridge::CvImage(image_msg->header,
00248 sensor_msgs::image_encodings::TYPE_32FC1,
00249 coefficient_image).toImageMsg());
00250
00251 cv::Mat threshold_image = cv::Mat::zeros(
00252 coefficient_image.rows, coefficient_image.cols, CV_32FC1);
00253 if (threshold_method_ == 0) {
00254 cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
00255 cv::THRESH_BINARY_INV);
00256 }
00257 else if (threshold_method_ == 1) {
00258 cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
00259 cv::THRESH_BINARY);
00260 }
00261 else if (threshold_method_ == 2 || threshold_method_ == 3) {
00262
00263 cv::Mat otsu_image = cv::Mat::zeros(
00264 coefficient_image.rows, coefficient_image.cols, CV_8UC1);
00265 cv::Mat otsu_result_image = cv::Mat::zeros(
00266 coefficient_image.rows, coefficient_image.cols, CV_8UC1);
00267 coefficient_image.convertTo(otsu_image, 8, 255.0);
00268 cv::threshold(otsu_image, otsu_result_image, coef_threshold_, 255,
00269 cv::THRESH_OTSU);
00270
00271 if (threshold_method_ == 2) {
00272 otsu_result_image.convertTo(threshold_image, 32, 1 / 255.0);
00273 }
00274 else if (threshold_method_ == 3) {
00275 otsu_result_image.convertTo(threshold_image, 32, - 1 / 255.0, 1.0);
00276 }
00277 }
00278 cv::Mat threshold_uchar_image = cv::Mat(threshold_image.rows,
00279 threshold_image.cols,
00280 CV_8UC1);
00281 threshold_image.convertTo(threshold_uchar_image, 8, 255.0);
00282
00283 pub_result_.publish(
00284 cv_bridge::CvImage(image_msg->header,
00285 sensor_msgs::image_encodings::MONO8,
00286 threshold_uchar_image).toImageMsg());
00287 }
00288
00289 double ColorHistogramLabelMatch::coefficients(
00290 const cv::Mat& ref_hist,
00291 const cv::Mat& target_hist)
00292 {
00293 if (coefficient_method_ == 0) {
00294 return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0;
00295 }
00296 else if (coefficient_method_ == 1) {
00297 double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR);
00298 return 1/ (x * x + 1);
00299 }
00300 else if (coefficient_method_ == 2) {
00301 return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT);
00302 }
00303 else if (coefficient_method_ == 3) {
00304 return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA);
00305 }
00306 else if (coefficient_method_ == 4 || coefficient_method_ == 5) {
00307 cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
00308 cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
00309
00310 for (size_t i = 0; i < ref_hist.cols; i++) {
00311 ref_sig.at<float>(i, 0) = ref_hist.at<float>(0, i);
00312 target_sig.at<float>(i, 0) = target_hist.at<float>(0, i);
00313 ref_sig.at<float>(i, 1) = i;
00314 target_sig.at<float>(i, 1) = i;
00315 }
00316 if (coefficient_method_ == 4) {
00317 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1);
00318 return 1.0 / (1.0 + x * x);
00319 }
00320 else {
00321 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2);
00322 return 1.0 / (1.0 + x * x);
00323 }
00324 }
00325 else {
00326 NODELET_ERROR("unknown coefficiet method: %d", coefficient_method_);
00327 return 0;
00328 }
00329 }
00330
00331 void ColorHistogramLabelMatch::histogramCallback(
00332 const jsk_recognition_msgs::ColorHistogram::ConstPtr& histogram_msg)
00333 {
00334 boost::mutex::scoped_lock lock(mutex_);
00335
00336 histogram_ = cv::Mat(1, histogram_msg->histogram.size(), CV_32FC1);
00337 for (size_t i = 0; i < histogram_msg->histogram.size(); i++) {
00338 histogram_.at<float>(0, i) = histogram_msg->histogram[i];
00339 }
00340 cv::normalize(histogram_, histogram_, 1, histogram_.rows, cv::NORM_L2,
00341 -1, cv::Mat());
00342 }
00343
00344 void ColorHistogramLabelMatch::configCallback(
00345 Config &config, uint32_t level)
00346 {
00347 boost::mutex::scoped_lock lock(mutex_);
00348 coefficient_method_ = config.coefficient_method;
00349 max_value_ = config.max_value;
00350 min_value_ = config.min_value;
00351 masked_coefficient_ = config.masked_coefficient;
00352 coef_threshold_ = config.coef_threshold;
00353 threshold_method_ = config.threshold_method;
00354 }
00355
00356 }
00357
00358 #include <pluginlib/class_list_macros.h>
00359 PLUGINLIB_EXPORT_CLASS (jsk_perception::ColorHistogramLabelMatch, nodelet::Nodelet);