color_histogram_label_match.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
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); // BGR8
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       // get label_mask & whole_mask and check is it all black or not
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         //cv::Mat hist_mat = hist;
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     // apply threshold operation
00244     cv::Mat threshold_image = cv::Mat::zeros(
00245       coefficient_image.rows, coefficient_image.cols, CV_32FC1);
00246     if (threshold_method_ == 0) { // smaller than
00247       cv::threshold(coefficient_image, threshold_image, coef_threshold_, 1,
00248                    cv::THRESH_BINARY_INV);
00249     }
00250     else if (threshold_method_ == 1) { // greater than
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       // convert image into 8UC to apply otsu' method
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       // convert it into float image again
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     // convert image from float to uchar
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       //JSK_NODELET_INFO("ref_hist.cols = %d", ref_hist.cols);
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     //histogram_ = histogram_msg->histogram;
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);


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:36:15