color_histogram_matcher_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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_pcl_ros/color_histogram_matcher.h"
00037 #include <pluginlib/class_list_macros.h>
00038 #include <pcl/filters/extract_indices.h>
00039 #include <pcl/common/centroid.h>
00040 #include <geometry_msgs/PoseStamped.h>
00041 
00042 #include <pcl/pcl_config.h>
00043 #if PCL_VERSION_COMPARE (<, 1, 7, 2)
00044 #include <jsk_pcl_ros/pcl/point_types_conversion.h>
00045 #else
00046 #include <pcl/point_types_conversion.h>
00047 #endif
00048 
00049 namespace jsk_pcl_ros
00050 {
00051   void ColorHistogramMatcher::onInit()
00052   {
00053     ConnectionBasedNodelet::onInit();
00054     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00055     dynamic_reconfigure::Server<Config>::CallbackType f =
00056       boost::bind (&ColorHistogramMatcher::configCallback, this, _1, _2);
00057     srv_->setCallback (f);
00058 
00059     policy_ = USE_HUE_AND_SATURATION;
00060     reference_set_ = false;
00061     // setup publishers
00062     all_histogram_pub_
00063       = advertise<jsk_recognition_msgs::ColorHistogramArray>(
00064         *pnh_, "output_histograms", 1);
00065     best_pub_
00066       = advertise<geometry_msgs::PoseStamped>(*pnh_, "best_match", 1);
00067     reference_histogram_pub_
00068       = advertise<jsk_recognition_msgs::ColorHistogram>(*pnh_, "output_reference", 1);
00069     result_pub_
00070       = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output", 1);
00071     coefficient_points_pub_
00072       = advertise<sensor_msgs::PointCloud2>(*pnh_, "coefficient_points", 1);
00073     onInitPostProcess();
00074  }
00075 
00076   void ColorHistogramMatcher::subscribe()
00077   {
00078     reference_sub_ = pnh_->subscribe("input_reference_cloud", 1,
00079                                      &ColorHistogramMatcher::reference,
00080                                      this);
00081     reference_histogram_sub_ = pnh_->subscribe(
00082       "input_reference", 1,
00083       &ColorHistogramMatcher::referenceHistogram,
00084       this);
00085     sub_input_.subscribe(*pnh_, "input", 1);
00086     sub_indices_.subscribe(*pnh_, "input_indices", 1);
00087     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00088     sync_->connectInput(sub_input_, sub_indices_);
00089     sync_->registerCallback(boost::bind(&ColorHistogramMatcher::feature,
00090                                         this, _1, _2));
00091   }
00092 
00093   void ColorHistogramMatcher::unsubscribe()
00094   {
00095     reference_sub_.shutdown();
00096     reference_histogram_sub_.shutdown();
00097     sub_input_.unsubscribe();
00098     sub_indices_.unsubscribe();
00099   }
00100 
00101   void ColorHistogramMatcher::configCallback(Config &config, uint32_t level)
00102   {
00103     boost::mutex::scoped_lock lock(mutex_);
00104     coefficient_thr_ = config.coefficient_thr;
00105     bin_size_ = config.bin_size;
00106     publish_colored_cloud_ = config.publish_colored_cloud;
00107     power_ = config.power;
00108     color_min_coefficient_ = config.color_min_coefficient;
00109     color_max_coefficient_ = config.color_max_coefficient;
00110     show_method_ = config.show_method;
00111     ComparePolicy new_histogram;
00112     if (config.histogram_method == 0) {
00113       new_histogram = USE_HUE;
00114     }
00115     else if (config.histogram_method == 1) {
00116       new_histogram = USE_SATURATION;
00117     }
00118     else if (config.histogram_method == 2) {
00119       new_histogram = USE_VALUE;
00120     }
00121     else if (config.histogram_method == 3) {
00122       new_histogram = USE_HUE_AND_SATURATION;
00123     }
00124     else {
00125       ROS_WARN("unknown histogram method");
00126       return;
00127     }
00128     if (new_histogram != policy_) {
00129       policy_ = new_histogram;
00130       reference_set_ = false;
00131       ROS_WARN("histogram method is reset, please specify histogram again");
00132     }
00133   }
00134 
00135   
00136   void ColorHistogramMatcher::feature(
00137       const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00138       const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& input_indices)
00139   {
00140     boost::mutex::scoped_lock lock(mutex_);
00141     if (!reference_set_) {
00142       NODELET_WARN("reference histogram is not available yet");
00143       return;
00144     }
00145     pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00146     pcl::fromROSMsg(*input_cloud, *pcl_cloud);
00147     pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud (new pcl::PointCloud<pcl::PointXYZHSV>);
00148     pcl::PointCloudXYZRGBtoXYZHSV(*pcl_cloud, *hsv_cloud);
00149     for (size_t i = 0; i < pcl_cloud->points.size(); i++) {
00150       hsv_cloud->points[i].x = pcl_cloud->points[i].x;
00151       hsv_cloud->points[i].y = pcl_cloud->points[i].y;
00152       hsv_cloud->points[i].z = pcl_cloud->points[i].z;
00153     }
00154     // compute histograms first
00155     std::vector<std::vector<float> > histograms;
00156     histograms.resize(input_indices->cluster_indices.size());
00157     unsigned long point_all_size=0;
00158     pcl::ExtractIndices<pcl::PointXYZHSV> extract;
00159     extract.setInputCloud(hsv_cloud);
00160     // for debug
00161     jsk_recognition_msgs::ColorHistogramArray histogram_array;
00162     histogram_array.header = input_cloud->header;
00163     std::vector<pcl::PointCloud<pcl::PointXYZHSV>::Ptr > segmented_clouds;
00164     for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
00165       pcl::IndicesPtr indices (new std::vector<int>(input_indices->cluster_indices[i].indices));
00166       point_all_size+=indices->size();
00167       extract.setIndices(indices);
00168       pcl::PointCloud<pcl::PointXYZHSV> segmented_cloud;
00169       extract.filter(segmented_cloud);
00170       segmented_clouds.push_back(segmented_cloud.makeShared());
00171       std::vector<float> histogram;
00172       computeHistogram(segmented_cloud, histogram, policy_);
00173       histograms[i] = histogram;
00174       jsk_recognition_msgs::ColorHistogram ros_histogram;
00175       ros_histogram.header = input_cloud->header;
00176       ros_histogram.histogram = histogram;
00177       histogram_array.histograms.push_back(ros_histogram);
00178     }
00179     all_histogram_pub_.publish(histogram_array);
00180 
00181     // compare histograms
00182     jsk_recognition_msgs::ClusterPointIndices result;
00183     result.header = input_indices->header;
00184     double best_coefficient = - DBL_MAX;
00185     int best_index = -1;
00186     pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00187     if(publish_colored_cloud_){
00188       output_cloud->width=point_all_size;
00189       output_cloud->height=1; 
00190       output_cloud->resize(point_all_size);
00191     }
00192     unsigned long count_points=0;
00193     for (size_t i = 0; i < input_indices->cluster_indices.size(); i++) {
00194       const double coefficient = bhattacharyyaCoefficient(histograms[i], reference_histogram_);
00195       NODELET_DEBUG_STREAM("coefficient: " << i << "::" << coefficient);
00196       if(publish_colored_cloud_){
00197         int tmp_point_size = input_indices->cluster_indices[i].indices.size();
00198         double color_standard;
00199         if(color_min_coefficient_ > coefficient){
00200           color_standard = 0;
00201         } else if(color_max_coefficient_ < coefficient){
00202           color_standard = 1;
00203         } else{
00204           color_standard = (coefficient - color_min_coefficient_) / (color_max_coefficient_ - color_min_coefficient_);
00205         }
00206         double color_standard_powered = 1;
00207         for (int k=0; k<power_; k++){
00208           color_standard_powered *= color_standard;
00209         }
00210         unsigned char color_r, color_g, color_b;
00211         switch(show_method_){
00212         case 0:
00213           color_r=(int)(255*color_standard_powered);
00214           color_g=0;
00215           color_b=(int)(255*(1-color_standard_powered));
00216           break;
00217         case 1:
00218           // like thermo
00219           int color_index = (int)(color_standard_powered*1280);
00220           switch(color_index/256){
00221           case 0:
00222             color_r=0; color_g=0; color_b=color_index;
00223             break;
00224           case 1:
00225           color_r=color_index-256; color_g=0; color_b=255;
00226           break;
00227           case 2:
00228             color_r=255; color_g=0; color_b=255-(color_index-256*2);
00229             break;
00230           case 3:
00231             color_r=255; color_g=color_index-256*3; color_b=0;
00232             break;
00233           case 4:
00234             color_r=255; color_g=255; color_b=color_index-256*4;
00235             break;
00236           case 5:
00237             color_r=255; color_g=255; color_b=255;
00238             break;
00239           }
00240           break;
00241         }
00242         for(int j=0; j<tmp_point_size; j++){
00243           output_cloud->points[j+count_points].x=segmented_clouds[i]->points[j].x;
00244           output_cloud->points[j+count_points].y=segmented_clouds[i]->points[j].y;
00245           output_cloud->points[j+count_points].z=segmented_clouds[i]->points[j].z;
00246           output_cloud->points[j+count_points].r=color_r;
00247           output_cloud->points[j+count_points].g=color_g;
00248           output_cloud->points[j+count_points].b=color_b;
00249         }
00250         count_points+=tmp_point_size;
00251       }
00252       if (coefficient > coefficient_thr_) {
00253         result.cluster_indices.push_back(input_indices->cluster_indices[i]);
00254         if (best_coefficient < coefficient) {
00255           best_coefficient = coefficient;
00256           best_index = i;
00257         }
00258       }
00259     }
00260     NODELET_DEBUG("best coefficients: %f, %d", best_coefficient, best_index);
00261     //show coefficience with points
00262     sensor_msgs::PointCloud2 p_msg;
00263     if(publish_colored_cloud_){
00264       pcl::toROSMsg(*output_cloud, p_msg);
00265       p_msg.header=input_cloud->header;
00266       coefficient_points_pub_.publish(p_msg);
00267     }
00268     result_pub_.publish(result);
00269     if (best_index != -1) {
00270       pcl::PointCloud<pcl::PointXYZHSV>::Ptr best_cloud
00271         = segmented_clouds[best_index];
00272       Eigen::Vector4f center;
00273       pcl::compute3DCentroid(*best_cloud, center);
00274       geometry_msgs::PoseStamped best_pose;
00275       best_pose.header = input_cloud->header;
00276       best_pose.pose.position.x = center[0];
00277       best_pose.pose.position.y = center[1];
00278       best_pose.pose.position.z = center[2];
00279       best_pose.pose.orientation.w = 1.0;
00280       best_pub_.publish(best_pose);
00281     }
00282   }
00283 
00284   double ColorHistogramMatcher::bhattacharyyaCoefficient(const std::vector<float>& a, const std::vector<float>& b)
00285   {
00286     if (a.size() != b.size()) {
00287       NODELET_ERROR("the bin size of histograms do not match");
00288       return 0.0;
00289     }
00290     double sum = 0.0;
00291     for (size_t i = 0; i < a.size(); i++) {
00292       sum += sqrt(a[i] * b[i]);
00293     }
00294     return sum;
00295   }
00296   
00297   void ColorHistogramMatcher::computeHistogram(
00298     const pcl::PointCloud<pcl::PointXYZHSV>& cloud,
00299     std::vector<float>& output,
00300     const ComparePolicy policy)
00301   {
00302     if (policy == USE_HUE_AND_SATURATION) {
00303       std::vector<float> hue, saturation;
00304       computeHistogram(cloud, hue, USE_HUE);
00305       computeHistogram(cloud, saturation, USE_SATURATION);
00306       
00307       output.resize(hue.size() + saturation.size());
00308       for (size_t i = 0; i < hue.size(); i++) {
00309         output[i] = hue[i];
00310       }
00311       for (size_t j = hue.size(); j < hue.size() + saturation.size(); j++) {
00312         output[j] = saturation[j - hue.size()];
00313       }
00314     }
00315     else {
00316       double val_max, val_min;
00317       if (policy == USE_HUE) {
00318         val_max = 360.0;
00319         val_min = 0.0;
00320       }
00321       else {
00322         val_max = 1.0;
00323         val_min = 0.0;
00324       }
00325       output.resize(bin_size_, 0);
00326       for (size_t i = 0; i < cloud.points.size(); i++) {
00327         pcl::PointXYZHSV output_point = cloud.points[i];
00328         // ratil
00329         double val;
00330         if (policy == USE_HUE) {
00331           val = output_point.h;
00332         }
00333         else if (policy == USE_SATURATION) {
00334           val = output_point.s;
00335         }
00336         else if (policy == USE_VALUE) {
00337           val = output_point.v;
00338         }
00339         int index = int((val - val_min) / (val_max - val_min) * bin_size_);
00340         if (index >= bin_size_) {
00341           index = bin_size_ - 1;
00342         }
00343         output[index] += 1.0;
00344       }
00345     }
00346     // normalize
00347     double sum = 0;
00348     for (size_t i = 0; i < output.size(); i++) {
00349       sum += output[i];
00350     }
00351     for (size_t i = 0; i < output.size(); i++) {
00352       if (sum != 0.0) {
00353         output[i] /= sum;
00354       }
00355       else {
00356         output[i] = 0.0;
00357       }
00358     }
00359   }
00360   
00361   void ColorHistogramMatcher::reference(
00362     const sensor_msgs::PointCloud2::ConstPtr& input_cloud)
00363   {
00364     boost::mutex::scoped_lock lock(mutex_);
00365     std::vector<float> hist;
00366     pcl::PointCloud<pcl::PointXYZRGB> pcl_cloud;
00367     pcl::fromROSMsg(*input_cloud, pcl_cloud);
00368     pcl::PointCloud<pcl::PointXYZHSV> hsv_cloud;
00369     pcl::PointCloudXYZRGBtoXYZHSV(pcl_cloud, hsv_cloud);
00370     computeHistogram(hsv_cloud, hist, policy_);
00371     reference_histogram_ = hist;
00372     NODELET_INFO("update reference");
00373     reference_set_ = true;
00374     jsk_recognition_msgs::ColorHistogram ros_histogram;
00375     ros_histogram.header = input_cloud->header;
00376     ros_histogram.histogram = reference_histogram_;
00377     reference_histogram_pub_.publish(ros_histogram);
00378   }
00379   
00380   void ColorHistogramMatcher::referenceHistogram(
00381     const jsk_recognition_msgs::ColorHistogram::ConstPtr& input_histogram)
00382   {
00383     boost::mutex::scoped_lock lock(mutex_);
00384     NODELET_INFO("update reference");
00385     reference_histogram_ = input_histogram->histogram;
00386     reference_histogram_pub_.publish(input_histogram);
00387     reference_set_ = true;
00388   }
00389   
00390 }
00391 
00392 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ColorHistogramMatcher,
00393                         nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49