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 #include "jsk_perception/filter_mask_image_with_size.h"
00036 #include <boost/assign.hpp>
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <opencv2/opencv.hpp>
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <sensor_msgs/image_encodings.h>
00041
00042 namespace jsk_perception
00043 {
00044 void FilterMaskImageWithSize::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 pnh_->param("approximate_sync", approximate_sync_, false);
00048 pnh_->param("queue_size", queue_size_, 100);
00049 pnh_->param("use_reference", use_reference_, false);
00050 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> >(*pnh_);
00051 dynamic_reconfigure::Server<Config>::CallbackType f =
00052 boost::bind(&FilterMaskImageWithSize::configCallback, this, _1, _2);
00053 srv_->setCallback(f);
00054 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00055 onInitPostProcess();
00056 }
00057
00058 void FilterMaskImageWithSize::configCallback(
00059 Config &config, uint32_t level)
00060 {
00061 boost::mutex::scoped_lock lock(mutex_);
00062 min_size_ = config.min_size;
00063 max_size_ = config.max_size;
00064 if (use_reference_)
00065 {
00066 min_relative_size_ = config.min_relative_size;
00067 max_relative_size_ = config.max_relative_size;
00068 }
00069 else
00070 {
00071 if ((config.min_relative_size != 0) || (config.max_relative_size != 1))
00072 {
00073 ROS_WARN("Rosparam ~min_relative_size and ~max_relative_size is enabled only with ~use_reference is true,"
00074 " and will be overwritten by 0 and 1.");
00075 }
00076 min_relative_size_ = config.min_relative_size = 0;
00077 max_relative_size_ = config.max_relative_size = 1;
00078 }
00079 }
00080
00081 void FilterMaskImageWithSize::subscribe()
00082 {
00083 sub_input_.subscribe(*pnh_, "input", 1);
00084 ros::V_string names = boost::assign::list_of("~input");
00085 if (use_reference_)
00086 {
00087 sub_reference_.subscribe(*pnh_, "input/reference", 1);
00088 if (approximate_sync_)
00089 {
00090 async_ = boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicy> >(queue_size_);
00091 async_->connectInput(sub_input_, sub_reference_);
00092 async_->registerCallback(boost::bind(&FilterMaskImageWithSize::filter, this, _1, _2));
00093 }
00094 else
00095 {
00096 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00097 sync_->connectInput(sub_input_, sub_reference_);
00098 sync_->registerCallback(boost::bind(&FilterMaskImageWithSize::filter, this, _1, _2));
00099 }
00100 names.push_back("~input/reference");
00101 }
00102 else
00103 {
00104 sub_input_.registerCallback(&FilterMaskImageWithSize::filter, this);
00105 }
00106 jsk_topic_tools::warnNoRemap(names);
00107 }
00108
00109 void FilterMaskImageWithSize::unsubscribe()
00110 {
00111 sub_input_.unsubscribe();
00112 if (use_reference_)
00113 {
00114 sub_reference_.unsubscribe();
00115 }
00116 }
00117
00118 void FilterMaskImageWithSize::filter(
00119 const sensor_msgs::Image::ConstPtr& input_msg)
00120 {
00121 filter(input_msg, input_msg);
00122 }
00123
00124 void FilterMaskImageWithSize::filter(
00125 const sensor_msgs::Image::ConstPtr& input_msg,
00126 const sensor_msgs::Image::ConstPtr& reference_msg)
00127 {
00128 if ((input_msg->height != reference_msg->height) ||
00129 (input_msg->width != reference_msg->width))
00130 {
00131 ROS_FATAL("Input mask size must match. input: (%d, %d), reference: (%d, %d)",
00132 input_msg->height, input_msg->width,
00133 reference_msg->height, reference_msg->width);
00134 return;
00135 }
00136 cv::Mat input = cv_bridge::toCvShare(input_msg, input_msg->encoding)->image;
00137 cv::Mat reference = cv_bridge::toCvShare(reference_msg, reference_msg->encoding)->image;
00138 double size_image = input_msg->height * input_msg->width;
00139 double size_input = cv::countNonZero(input > 127) / size_image;
00140 double size_reference = cv::countNonZero(reference > 127) / size_image;
00141 double size_relative = size_input / size_reference;
00142 ROS_INFO("image relative: %lf <= %lf <= %lf, mask relative: %lf <= %lf <= %lf",
00143 min_size_, size_input, max_size_,
00144 min_relative_size_, size_relative, max_relative_size_);
00145 if (!std::isnan(size_relative) &&
00146 (min_size_ <= size_input) && (size_input <= max_size_) &&
00147 (min_relative_size_ <= size_relative) && (size_relative <= max_relative_size_))
00148 {
00149 pub_.publish(input_msg);
00150 }
00151 }
00152 }
00153
00154 #include <pluginlib/class_list_macros.h>
00155 PLUGINLIB_EXPORT_CLASS (jsk_perception::FilterMaskImageWithSize, nodelet::Nodelet);