37 #include <boost/assign.hpp> 47 DiagnosticNodelet::onInit();
48 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
49 dynamic_reconfigure::Server<Config>::CallbackType
f =
52 srv_->setCallback (f);
53 pnh_->param(
"use_mask",
use_mask_,
false);
57 *
pnh_,
"output/coefficient_image", 1);
59 *
pnh_,
"output/extracted_region", 1);
67 ros::V_string names = boost::assign::list_of(
"~input")(
"~input/label");
69 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
71 names.push_back(
"~input/mask");
73 sync_->registerCallback(
78 sync_wo_mask_ = boost::make_shared<message_filters::Synchronizer<SyncPolicyWithoutMask> >(100);
86 names.push_back(
"~input/histogram");
101 std::vector<int>& keys)
103 std::map<int, bool> map;
104 for (
int j = 0; j < label.rows; j++) {
105 for (
int i = 0; i < label.cols; i++) {
106 int label_value = label.at<
int>(j, i);
107 if (map.find(label_value) == map.end()) {
108 map[label_value] =
true;
113 for (std::map<int, bool>::iterator it = map.begin();
116 keys.push_back(it->first);
124 for (
int j = 0; j < label_image.rows; j++) {
125 for (
int i = 0; i < label_image.cols; i++) {
126 if (label_image.at<
int>(j, i) == label) {
127 mask.at<uchar>(j, i) = 255;
134 const cv::Mat& original_image,
135 const cv::Mat& masked_image)
137 int original_count = 0;
138 int masked_count = 0;
139 for (
int j = 0; j < original_image.rows; j++) {
140 for (
int i = 0; i < original_image.cols; i++) {
141 if (original_image.at<uchar>(j, i) != 0) {
144 if (masked_image.at<uchar>(j, i) != 0) {
149 return original_count != masked_count;
153 const sensor_msgs::Image::ConstPtr& image_msg,
154 const sensor_msgs::Image::ConstPtr& label_msg)
156 cv::Mat whole_mask = cv::Mat(image_msg->height,
157 image_msg->width, CV_8UC1, cv::Scalar(255));
158 sensor_msgs::Image::ConstPtr mask_msg
162 match(image_msg, label_msg, mask_msg);
167 const sensor_msgs::Image::ConstPtr& image_msg,
168 const sensor_msgs::Image::ConstPtr& label_msg,
169 const sensor_msgs::Image::ConstPtr& mask_msg)
184 cv::Mat coefficient_image = cv::Mat::zeros(
185 image_msg->height, image_msg->width, CV_32FC1);
186 std::vector<int> labels;
189 cv::Mat coefficients_heat_image = cv::Mat::zeros(
190 image_msg->height, image_msg->width, CV_8UC3);
193 const float* hist_range = { range };
194 double min_coef = DBL_MAX;
195 double max_coef = - DBL_MAX;
196 for (
size_t i = 0; i < labels.size(); i++) {
197 int label_index = labels[i];
198 cv::Mat label_mask = cv::Mat::zeros(label.rows, label.cols, CV_8UC1);
202 cv::Mat masked_label;
203 label_mask.copyTo(masked_label, whole_mask);
204 if (
isMasked(label_mask, masked_label)) {
209 bool uniform =
true;
bool accumulate =
false;
210 cv::calcHist(&image, 1, 0, label_mask, hist, 1,
211 &hist_size, &hist_range, uniform, accumulate);
212 cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2, -1,
214 cv::Mat hist_mat = cv::Mat::zeros(1, hist_size, CV_32FC1);
215 for (
size_t j = 0; j < hist_size; j++) {
216 hist_mat.at<
float>(0, j) = hist.at<
float>(0, j);
221 if (min_coef > coef) {
224 if (max_coef < coef) {
229 for (
size_t j = 0; j < coefficients_heat_image.rows; j++) {
230 for (
size_t i = 0; i < coefficients_heat_image.cols; i++) {
231 if (label_mask.at<uchar>(j, i) == 255) {
232 coefficients_heat_image.at<cv::Vec3b>(j, i)
233 = cv::Vec3b(
int(coef_color.b * 255),
234 int(coef_color.g * 255),
235 int(coef_color.r * 255));
236 coefficient_image.at<
float>(j, i) = coef;
251 cv::Mat threshold_image = cv::Mat::zeros(
252 coefficient_image.rows, coefficient_image.cols, CV_32FC1);
255 cv::THRESH_BINARY_INV);
263 cv::Mat otsu_image = cv::Mat::zeros(
264 coefficient_image.rows, coefficient_image.cols, CV_8UC1);
265 cv::Mat otsu_result_image = cv::Mat::zeros(
266 coefficient_image.rows, coefficient_image.cols, CV_8UC1);
267 coefficient_image.convertTo(otsu_image, 8, 255.0);
272 otsu_result_image.convertTo(threshold_image, 32, 1 / 255.0);
275 otsu_result_image.convertTo(threshold_image, 32, - 1 / 255.0, 1.0);
278 cv::Mat threshold_uchar_image = cv::Mat(threshold_image.rows,
279 threshold_image.cols,
281 threshold_image.convertTo(threshold_uchar_image, 8, 255.0);
290 const cv::Mat& ref_hist,
291 const cv::Mat& target_hist)
294 return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0;
297 double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR);
298 return 1/ (x * x + 1);
301 return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT);
304 return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA);
307 cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
308 cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1);
310 for (
size_t i = 0; i < ref_hist.cols; i++) {
311 ref_sig.at<
float>(i, 0) = ref_hist.at<
float>(0, i);
312 target_sig.at<
float>(i, 0) = target_hist.at<
float>(0, i);
313 ref_sig.at<
float>(i, 1) = i;
314 target_sig.at<
float>(i, 1) = i;
317 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1);
318 return 1.0 / (1.0 + x * x);
321 double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2);
322 return 1.0 / (1.0 + x * x);
332 const jsk_recognition_msgs::ColorHistogram::ConstPtr& histogram_msg)
336 histogram_ = cv::Mat(1, histogram_msg->histogram.size(), CV_32FC1);
337 for (
size_t i = 0; i < histogram_msg->histogram.size(); i++) {
338 histogram_.at<
float>(0, i) = histogram_msg->histogram[i];
345 Config &config, uint32_t level)
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
message_filters::Subscriber< sensor_msgs::Image > sub_mask_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
message_filters::Subscriber< sensor_msgs::Image > sub_label_
virtual void getLabels(const cv::Mat &label, std::vector< int > &labels)
ros::Publisher pub_coefficient_image_
virtual double coefficients(const cv::Mat &ref_hist, const cv::Mat &target_hist)
virtual void getMaskImage(const cv::Mat &label_image, const int label, cv::Mat &mask)
std::vector< std::string > V_string
virtual void unsubscribe()
ros::Subscriber sub_histogram_
message_filters::Subscriber< sensor_msgs::Image > sub_image_
virtual void histogramCallback(const jsk_recognition_msgs::ColorHistogram::ConstPtr &histogram_msg)
virtual void match(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::Image::ConstPtr &label_msg, const sensor_msgs::Image::ConstPtr &mask_msg)
const std::string TYPE_32FC1
virtual void configCallback(Config &config, uint32_t level)
ros::Publisher pub_result_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicyWithoutMask > > sync_wo_mask_
ros::Publisher pub_debug_
ColorHistogramLabelMatchConfig Config
#define NODELET_INFO(...)
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)
float masked_coefficient_
virtual bool isMasked(const cv::Mat &original_image, const cv::Mat &masked_image)
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
PLUGINLIB_EXPORT_CLASS(jsk_perception::ColorHistogramLabelMatch, nodelet::Nodelet)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_