36 #include <boost/assign.hpp> 38 #include <opencv2/opencv.hpp> 46 DiagnosticNodelet::onInit();
50 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> >(*pnh_);
51 dynamic_reconfigure::Server<Config>::CallbackType
f =
54 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
59 Config &config, uint32_t level)
71 if ((config.min_relative_size != 0) || (config.max_relative_size != 1))
73 ROS_WARN(
"Rosparam ~min_relative_size and ~max_relative_size is enabled only with ~use_reference is true," 74 " and will be overwritten by 0 and 1.");
90 async_ = boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicy> >(
queue_size_);
96 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
100 names.push_back(
"~input/reference");
119 const sensor_msgs::Image::ConstPtr& input_msg)
121 filter(input_msg, input_msg);
125 const sensor_msgs::Image::ConstPtr& input_msg,
126 const sensor_msgs::Image::ConstPtr& reference_msg)
128 if ((input_msg->height != reference_msg->height) ||
129 (input_msg->width != reference_msg->width))
131 ROS_FATAL(
"Input mask size must match. input: (%d, %d), reference: (%d, %d)",
132 input_msg->height, input_msg->width,
133 reference_msg->height, reference_msg->width);
138 double size_image = input_msg->height * input_msg->width;
139 double size_input = cv::countNonZero(input > 127) / size_image;
140 double size_reference = cv::countNonZero(reference > 127) / size_image;
141 double size_relative = size_input / size_reference;
142 ROS_INFO(
"image relative: %lf <= %lf <= %lf, mask relative: %lf <= %lf <= %lf",
145 if (!std::isnan(size_relative) &&
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
PLUGINLIB_EXPORT_CLASS(jsk_perception::FilterMaskImageWithSize, nodelet::Nodelet)
virtual void configCallback(Config &config, uint32_t level)
jsk_perception::FilterMaskImageWithSizeConfig Config
boost::shared_ptr< message_filters::Synchronizer< ApproxSyncPolicy > > async_
double max_relative_size_
void publish(const boost::shared_ptr< M > &message) const
virtual void unsubscribe()
message_filters::Subscriber< sensor_msgs::Image > sub_reference_
std::vector< std::string > V_string
double min_relative_size_
message_filters::Subscriber< sensor_msgs::Image > sub_input_
virtual void filter(const sensor_msgs::Image::ConstPtr &input_msg)
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)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
Connection registerCallback(const C &callback)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_