38 #include <yaml-cpp/yaml.h> 45 DiagnosticNodelet::onInit();
49 std::string reference_file;
50 pnh_->param(
"reference_file", reference_file, std::string(
""));
53 ROS_INFO(
"Reading reference from %s", reference_file.c_str());
56 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
57 dynamic_reconfigure::Server<Config>::CallbackType
f =
60 srv_->setCallback (f);
61 pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output", 1);
81 sync_->registerCallback(
99 jsk_recognition_msgs::HistogramWithRange read_msg;
104 fin.open(file.c_str(), std::ifstream::in);
106 while (parser.GetNextDocument(doc)) {
107 const YAML::Node& bins_yaml = doc[
"bins"];
108 for (
size_t i = 0; i < bins_yaml.size(); i++) {
109 const YAML::Node& bin_yaml = bins_yaml[i];
110 const YAML::Node& min_value_yaml = bin_yaml[
"min_value"];
111 const YAML::Node& max_value_yaml = bin_yaml[
"max_value"];
112 const YAML::Node& count_yaml = bin_yaml[
"count"];
113 jsk_recognition_msgs::HistogramWithRangeBin bin;
114 min_value_yaml >> bin.min_value;
115 max_value_yaml >> bin.max_value;
116 count_yaml >> bin.count;
117 read_msg.bins.push_back(bin);
124 doc = YAML::LoadFile(file);
126 const YAML::Node& bins_yaml = doc[
"bins"];
127 for (
size_t i = 0; i < bins_yaml.size(); i++) {
128 const YAML::Node& bin_yaml = bins_yaml[i];
129 const YAML::Node& min_value_yaml = bin_yaml[
"min_value"];
130 const YAML::Node& max_value_yaml = bin_yaml[
"max_value"];
131 const YAML::Node& count_yaml = bin_yaml[
"count"];
132 jsk_recognition_msgs::HistogramWithRangeBin bin;
133 bin.min_value = min_value_yaml.as<
double> ();
134 bin.max_value = max_value_yaml.as<
double> ();
135 bin.count = count_yaml.as<
int> ();
136 read_msg.bins.push_back(bin);
142 reference_ = boost::make_shared<jsk_recognition_msgs::HistogramWithRange>(read_msg);
155 const jsk_recognition_msgs::HistogramWithRange::ConstPtr& ref_msg)
162 Config &config, uint32_t level)
169 const cv::MatND& ref_hist,
170 const cv::MatND& target_hist)
173 return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0;
176 double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR);
177 return 1/ (x * x + 1);
180 return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT);
183 return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA);
186 cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
187 cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
189 for (
size_t i = 0; i < ref_hist.cols; i++) {
190 ref_sig.at<
float>(i, 0) = ref_hist.at<
float>(0, i);
191 target_sig.at<
float>(i, 0) = target_hist.at<
float>(0, i);
192 ref_sig.at<
float>(i, 1) = i;
193 target_sig.at<
float>(i, 1) = i;
196 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1);
197 return 1.0 / (1.0 + x * x);
200 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2);
201 return 1.0 / (1.0 + x * x);
211 const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
212 const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr& histogram_msg)
218 if (polygon_msg->polygons.size() != histogram_msg->histograms.size()) {
219 NODELET_ERROR(
"length of polygon and histogram are not same");
222 cv::MatND reference_histogram
225 cv::normalize(reference_histogram, reference_histogram, 1, reference_histogram.rows, cv::NORM_L2,
227 jsk_recognition_msgs::PolygonArray new_msg(*polygon_msg);
228 for (
size_t i = 0; i < new_msg.polygons.size(); i++) {
231 histogram_msg->histograms[i].bins);
232 cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2,
235 if (polygon_msg->likelihood.size() == 0) {
236 new_msg.likelihood.push_back(d);
239 new_msg.likelihood[i] *=
d;
PLUGINLIB_EXPORT_CLASS(jsk_perception::PolygonArrayColorLikelihood, nodelet::Nodelet)
virtual void likelihood(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygon_msg, const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr &histogram_msg)
#define NODELET_ERROR(...)
virtual void referenceCallback(const jsk_recognition_msgs::HistogramWithRange::ConstPtr &ref_msg)
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygon_
void publish(const boost::shared_ptr< M > &message) const
jsk_recognition_msgs::HistogramWithRange::ConstPtr reference_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void configCallback(Config &config, uint32_t level)
cv::MatND HistogramWithRangeBinArrayTocvMatND(const std::vector< jsk_recognition_msgs::HistogramWithRangeBin > &histogram)
PolygonArrayColorLikelihoodConfig Config
bool reference_from_file_
virtual void unsubscribe()
message_filters::Subscriber< jsk_recognition_msgs::HistogramWithRangeArray > sub_histogram_
virtual void readReference(const std::string &file)
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
virtual double compareHist(const cv::MatND &ref_hist, const cv::MatND &target_hist)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
ros::Subscriber sub_reference_
#define ROS_ERROR_STREAM(args)