37 #include <boost/assign.hpp> 40 #include <opencv2/opencv.hpp> 47 DiagnosticNodelet::onInit();
50 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
59 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(
queue_size_);
64 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(
queue_size_);
68 ros::V_string names = boost::assign::list_of(
"~input/image")(
"~input/rect_array");
79 const sensor_msgs::Image::ConstPtr& image_msg,
80 const jsk_recognition_msgs::RectArray::ConstPtr& rects_msg)
83 cv::Mat density_image = cv::Mat::zeros(image_msg->height, image_msg->width, CV_32FC1);
86 for (
size_t k=0; k<rects_msg->rects.size(); k++) {
87 jsk_recognition_msgs::Rect rect = rects_msg->rects[k];
88 for (
int j=rect.y; j<(rect.y + rect.height); j++) {
89 for (
int i=rect.x; i<(rect.x + rect.width); i++) {
90 density_image.at<
float>(j, i)++;
96 double min_image_value;
97 double max_image_value;
98 cv::minMaxLoc(density_image, &min_image_value, &max_image_value);
99 cv::Mat(density_image - min_image_value).convertTo(
100 density_image, CV_32FC1, 1. / (max_image_value - min_image_value));
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
void publish(const boost::shared_ptr< M > &message) const
virtual void convert(const sensor_msgs::Image::ConstPtr &image_msg, const jsk_recognition_msgs::RectArray::ConstPtr &rects_msg)
PLUGINLIB_EXPORT_CLASS(jsk_perception::RectArrayToDensityImage, nodelet::Nodelet)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
std::vector< std::string > V_string
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)
sensor_msgs::ImagePtr toImageMsg() const
message_filters::Subscriber< jsk_recognition_msgs::RectArray > sub_rects_
virtual void unsubscribe()