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_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
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
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
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
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
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
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
00273 best_cloud->is_dense = false;
00274 Eigen::Vector4f center;
00275 pcl::compute3DCentroid(*best_cloud, center);
00276 geometry_msgs::PoseStamped best_pose;
00277 best_pose.header = input_cloud->header;
00278 best_pose.pose.position.x = center[0];
00279 best_pose.pose.position.y = center[1];
00280 best_pose.pose.position.z = center[2];
00281 best_pose.pose.orientation.w = 1.0;
00282 best_pub_.publish(best_pose);
00283 }
00284 }
00285
00286 double ColorHistogramMatcher::bhattacharyyaCoefficient(const std::vector<float>& a, const std::vector<float>& b)
00287 {
00288 if (a.size() != b.size()) {
00289 NODELET_ERROR("the bin size of histograms do not match");
00290 return 0.0;
00291 }
00292 double sum = 0.0;
00293 for (size_t i = 0; i < a.size(); i++) {
00294 sum += sqrt(a[i] * b[i]);
00295 }
00296 return sum;
00297 }
00298
00299 void ColorHistogramMatcher::computeHistogram(
00300 const pcl::PointCloud<pcl::PointXYZHSV>& cloud,
00301 std::vector<float>& output,
00302 const ComparePolicy policy)
00303 {
00304 if (policy == USE_HUE_AND_SATURATION) {
00305 std::vector<float> hue, saturation;
00306 computeHistogram(cloud, hue, USE_HUE);
00307 computeHistogram(cloud, saturation, USE_SATURATION);
00308
00309 output.resize(hue.size() + saturation.size());
00310 for (size_t i = 0; i < hue.size(); i++) {
00311 output[i] = hue[i];
00312 }
00313 for (size_t j = hue.size(); j < hue.size() + saturation.size(); j++) {
00314 output[j] = saturation[j - hue.size()];
00315 }
00316 }
00317 else {
00318 double val_max, val_min;
00319 if (policy == USE_HUE) {
00320 val_max = 360.0;
00321 val_min = 0.0;
00322 }
00323 else {
00324 val_max = 1.0;
00325 val_min = 0.0;
00326 }
00327 output.resize(bin_size_, 0);
00328 for (size_t i = 0; i < cloud.points.size(); i++) {
00329 pcl::PointXYZHSV output_point = cloud.points[i];
00330
00331 double val;
00332 if (policy == USE_HUE) {
00333 val = output_point.h;
00334 }
00335 else if (policy == USE_SATURATION) {
00336 val = output_point.s;
00337 }
00338 else if (policy == USE_VALUE) {
00339 val = output_point.v;
00340 }
00341 int index = int((val - val_min) / (val_max - val_min) * bin_size_);
00342 if (index >= bin_size_) {
00343 index = bin_size_ - 1;
00344 }
00345 output[index] += 1.0;
00346 }
00347 }
00348
00349 double sum = 0;
00350 for (size_t i = 0; i < output.size(); i++) {
00351 sum += output[i];
00352 }
00353 for (size_t i = 0; i < output.size(); i++) {
00354 if (sum != 0.0) {
00355 output[i] /= sum;
00356 }
00357 else {
00358 output[i] = 0.0;
00359 }
00360 }
00361 }
00362
00363 void ColorHistogramMatcher::reference(
00364 const sensor_msgs::PointCloud2::ConstPtr& input_cloud)
00365 {
00366 boost::mutex::scoped_lock lock(mutex_);
00367 std::vector<float> hist;
00368 pcl::PointCloud<pcl::PointXYZRGB> pcl_cloud;
00369 pcl::fromROSMsg(*input_cloud, pcl_cloud);
00370 pcl::PointCloud<pcl::PointXYZHSV> hsv_cloud;
00371 pcl::PointCloudXYZRGBtoXYZHSV(pcl_cloud, hsv_cloud);
00372 computeHistogram(hsv_cloud, hist, policy_);
00373 reference_histogram_ = hist;
00374 NODELET_INFO("update reference");
00375 reference_set_ = true;
00376 jsk_recognition_msgs::ColorHistogram ros_histogram;
00377 ros_histogram.header = input_cloud->header;
00378 ros_histogram.histogram = reference_histogram_;
00379 reference_histogram_pub_.publish(ros_histogram);
00380 }
00381
00382 void ColorHistogramMatcher::referenceHistogram(
00383 const jsk_recognition_msgs::ColorHistogram::ConstPtr& input_histogram)
00384 {
00385 boost::mutex::scoped_lock lock(mutex_);
00386 NODELET_INFO("update reference");
00387 reference_histogram_ = input_histogram->histogram;
00388 reference_histogram_pub_.publish(input_histogram);
00389 reference_set_ = true;
00390 }
00391
00392 }
00393
00394 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ColorHistogramMatcher,
00395 nodelet::Nodelet);