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