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/polygon_array_color_likelihood.h"
00037 #include <jsk_recognition_utils/cv_utils.h>
00038 #include <yaml-cpp/yaml.h>
00039 #include <fstream>
00040
00041 namespace jsk_perception
00042 {
00043 void PolygonArrayColorLikelihood::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pnh_->param("approximate_sync", approximate_sync_, false);
00047 pnh_->param("max_queue_size", max_queue_size_, 10);
00048 pnh_->param("synchronizer_queue_size", sync_queue_size_, 100);
00049 std::string reference_file;
00050 pnh_->param("reference_file", reference_file, std::string(""));
00051 reference_from_file_ = !reference_file.empty();
00052 if (reference_from_file_) {
00053 ROS_INFO("Reading reference from %s", reference_file.c_str());
00054 readReference(reference_file);
00055 }
00056 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00057 dynamic_reconfigure::Server<Config>::CallbackType f =
00058 boost::bind (
00059 &PolygonArrayColorLikelihood::configCallback, this, _1, _2);
00060 srv_->setCallback (f);
00061 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output", 1);
00062 }
00063
00064 void PolygonArrayColorLikelihood::subscribe()
00065 {
00066 if (!reference_from_file_) {
00067 sub_reference_ = pnh_->subscribe(
00068 "input/reference", 1, &PolygonArrayColorLikelihood::referenceCallback, this);
00069 }
00070 sub_polygon_.subscribe(*pnh_, "input/polygons", max_queue_size_);
00071 sub_histogram_.subscribe(*pnh_, "input/histograms", max_queue_size_);
00072 if (approximate_sync_) {
00073 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(sync_queue_size_);
00074 async_->connectInput(sub_polygon_, sub_histogram_);
00075 async_->registerCallback(
00076 boost::bind(&PolygonArrayColorLikelihood::likelihood, this, _1, _2));
00077 }
00078 else {
00079 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(sync_queue_size_);
00080 sync_->connectInput(sub_polygon_, sub_histogram_);
00081 sync_->registerCallback(
00082 boost::bind(&PolygonArrayColorLikelihood::likelihood, this, _1, _2));
00083 }
00084 }
00085
00086 void PolygonArrayColorLikelihood::readReference(const std::string& file)
00087 {
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099 jsk_recognition_msgs::HistogramWithRange read_msg;
00100 #ifdef USE_OLD_YAML
00101 YAML::Node doc;
00102 std::ifstream fin;
00103
00104 fin.open(file.c_str(), std::ifstream::in);
00105 YAML::Parser parser(fin);
00106 while (parser.GetNextDocument(doc)) {
00107 const YAML::Node& bins_yaml = doc["bins"];
00108 for (size_t i = 0; i < bins_yaml.size(); i++) {
00109 const YAML::Node& bin_yaml = bins_yaml[i];
00110 const YAML::Node& min_value_yaml = bin_yaml["min_value"];
00111 const YAML::Node& max_value_yaml = bin_yaml["max_value"];
00112 const YAML::Node& count_yaml = bin_yaml["count"];
00113 jsk_recognition_msgs::HistogramWithRangeBin bin;
00114 min_value_yaml >> bin.min_value;
00115 max_value_yaml >> bin.max_value;
00116 count_yaml >> bin.count;
00117 read_msg.bins.push_back(bin);
00118 }
00119 }
00120 #else
00121
00122 YAML::Node doc;
00123
00124 doc = YAML::LoadFile(file);
00125 if ( doc["bins"] ) {
00126 const YAML::Node& bins_yaml = doc["bins"];
00127 for (size_t i = 0; i < bins_yaml.size(); i++) {
00128 const YAML::Node& bin_yaml = bins_yaml[i];
00129 const YAML::Node& min_value_yaml = bin_yaml["min_value"];
00130 const YAML::Node& max_value_yaml = bin_yaml["max_value"];
00131 const YAML::Node& count_yaml = bin_yaml["count"];
00132 jsk_recognition_msgs::HistogramWithRangeBin bin;
00133 bin.min_value = min_value_yaml.as<double> ();
00134 bin.max_value = max_value_yaml.as<double> ();
00135 bin.count = count_yaml.as<int> ();
00136 read_msg.bins.push_back(bin);
00137 }
00138 } else {
00139 ROS_ERROR_STREAM("bins: keyword is not found in file(" << file << ")");
00140 }
00141 #endif
00142 reference_ = boost::make_shared<jsk_recognition_msgs::HistogramWithRange>(read_msg);
00143 }
00144
00145 void PolygonArrayColorLikelihood::unsubscribe()
00146 {
00147 if (!reference_from_file_) {
00148 sub_reference_.shutdown();
00149 }
00150 sub_polygon_.unsubscribe();
00151 sub_histogram_.unsubscribe();
00152 }
00153
00154 void PolygonArrayColorLikelihood::referenceCallback(
00155 const jsk_recognition_msgs::HistogramWithRange::ConstPtr& ref_msg)
00156 {
00157 boost::mutex::scoped_lock lock(mutex_);
00158 reference_ = ref_msg;
00159 }
00160
00161 void PolygonArrayColorLikelihood::configCallback(
00162 Config &config, uint32_t level)
00163 {
00164 boost::mutex::scoped_lock lock(mutex_);
00165 coefficient_method_ = config.coefficient_method;
00166 }
00167
00168 double PolygonArrayColorLikelihood::compareHist(
00169 const cv::MatND& ref_hist,
00170 const cv::MatND& target_hist)
00171 {
00172 if (coefficient_method_ == 0) {
00173 return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0;
00174 }
00175 else if (coefficient_method_ == 1) {
00176 double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR);
00177 return 1/ (x * x + 1);
00178 }
00179 else if (coefficient_method_ == 2) {
00180 return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT);
00181 }
00182 else if (coefficient_method_ == 3) {
00183 return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA);
00184 }
00185 else if (coefficient_method_ == 4 || coefficient_method_ == 5) {
00186 cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
00187 cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
00188
00189 for (size_t i = 0; i < ref_hist.cols; i++) {
00190 ref_sig.at<float>(i, 0) = ref_hist.at<float>(0, i);
00191 target_sig.at<float>(i, 0) = target_hist.at<float>(0, i);
00192 ref_sig.at<float>(i, 1) = i;
00193 target_sig.at<float>(i, 1) = i;
00194 }
00195 if (coefficient_method_ == 4) {
00196 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1);
00197 return 1.0 / (1.0 + x * x);
00198 }
00199 else {
00200 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2);
00201 return 1.0 / (1.0 + x * x);
00202 }
00203 }
00204 else {
00205 NODELET_ERROR("unknown coefficiet method: %d", coefficient_method_);
00206 return 0;
00207 }
00208 }
00209
00210 void PolygonArrayColorLikelihood::likelihood(
00211 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
00212 const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr& histogram_msg)
00213 {
00214 boost::mutex::scoped_lock lock(mutex_);
00215 if (!reference_) {
00216 return;
00217 }
00218 if (polygon_msg->polygons.size() != histogram_msg->histograms.size()) {
00219 NODELET_ERROR("length of polygon and histogram are not same");
00220 return;
00221 }
00222 cv::MatND reference_histogram
00223 = jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
00224 reference_->bins);
00225 cv::normalize(reference_histogram, reference_histogram, 1, reference_histogram.rows, cv::NORM_L2,
00226 -1, cv::Mat());
00227 jsk_recognition_msgs::PolygonArray new_msg(*polygon_msg);
00228 for (size_t i = 0; i < new_msg.polygons.size(); i++) {
00229 cv::MatND hist
00230 = jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
00231 histogram_msg->histograms[i].bins);
00232 cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2,
00233 -1, cv::Mat());
00234 double d = compareHist(reference_histogram, hist);
00235 if (polygon_msg->likelihood.size() == 0) {
00236 new_msg.likelihood.push_back(d);
00237 }
00238 else {
00239 new_msg.likelihood[i] *= d;
00240 }
00241 }
00242 pub_.publish(new_msg);
00243 }
00244 }
00245
00246 #include <pluginlib/class_list_macros.h>
00247 PLUGINLIB_EXPORT_CLASS (jsk_perception::PolygonArrayColorLikelihood,
00248 nodelet::Nodelet);