apply_mask_image.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
37 #include <boost/assign.hpp>
40 #include <opencv2/opencv.hpp>
41 #include <cv_bridge/cv_bridge.h>
43 
44 namespace jsk_perception
45 {
47  {
48  DiagnosticNodelet::onInit();
49  pnh_->param("approximate_sync", approximate_sync_, false);
50  pnh_->param("clip", clip_, true);
51  pnh_->param("negative", negative_, false);
52  pnh_->param("negative/before_clip", negative_before_clip_, true);
53  pnh_->param("mask_black_to_transparent", mask_black_to_transparent_, false);
54  pnh_->param("use_rectified_image", use_rectified_image_, true);
55  pnh_->param("queue_size", queue_size_, 100);
56  pnh_->param("cval", cval_, 0);
57  pub_image_ = advertise<sensor_msgs::Image>(
58  *pnh_, "output", 1);
59  pub_mask_ = advertise<sensor_msgs::Image>(
60  *pnh_, "output/mask", 1);
61  pub_camera_info_ = advertise<sensor_msgs::CameraInfo>(
62  *pnh_, "output/camera_info", 1);
64  }
65 
67  {
68  sub_image_.subscribe(*pnh_, "input", 1);
69  sub_mask_.subscribe(*pnh_, "input/mask", 1);
70  sub_info_ = pnh_->subscribe("input/camera_info", 1,
72  if (approximate_sync_) {
73  async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(queue_size_);
74  async_->connectInput(sub_image_, sub_mask_);
75  async_->registerCallback(boost::bind(&ApplyMaskImage::apply, this, _1, _2));
76  }
77  else {
78  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
79  sync_->connectInput(sub_image_, sub_mask_);
80  sync_->registerCallback(boost::bind(&ApplyMaskImage::apply, this, _1, _2));
81  }
82  ros::V_string names = boost::assign::list_of("~input")("~input/mask");
84  }
85 
87  {
90  }
91 
93  const sensor_msgs::CameraInfo::ConstPtr& info_msg)
94  {
95  boost::mutex::scoped_lock lock(mutex_);
96  camera_info_ = info_msg;
97  }
98 
100  const sensor_msgs::Image::ConstPtr& image_msg,
101  const sensor_msgs::Image::ConstPtr& mask_msg)
102  {
103  vital_checker_->poke();
104  cv::Mat image;
105  if (jsk_recognition_utils::isBGRA(image_msg->encoding)) {
106  cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
107  cv::cvtColor(tmp_image, image, cv::COLOR_BGRA2BGR);
108  }
109  else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) {
110  cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
111  cv::cvtColor(tmp_image, image, cv::COLOR_RGBA2BGR);
112  }
113  else { // BGR, RGB or GRAY
114  image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
115  }
116  cv::Mat mask = cv_bridge::toCvShare(mask_msg, "mono8")->image;
117  if (image.cols != mask.cols || image.rows != mask.rows) {
118  NODELET_ERROR("size of image and mask is different");
119  NODELET_ERROR("image: %dx%dx", image.cols, image.rows);
120  NODELET_ERROR("mask: %dx%dx", mask.cols, mask.rows);
121  return;
122  }
123 
125  cv::bitwise_not(mask, mask);
126  }
127 
128  cv::Rect region = jsk_recognition_utils::boundingRectOfMaskImage(mask);
129  if (camera_info_) {
130  sensor_msgs::CameraInfo camera_info(*camera_info_);
131  camera_info.header = image_msg->header;
132  camera_info.roi.x_offset = region.x;
133  camera_info.roi.y_offset = region.y;
134  camera_info.roi.width = region.width;
135  camera_info.roi.height = region.height;
136  camera_info.roi.do_rectify = use_rectified_image_;
137  pub_camera_info_.publish(camera_info);
138  }
139  if (clip_) {
140  mask = mask(region);
141  image = image(region);
142  }
143 
145  cv::bitwise_not(mask, mask);
146  }
147 
149  mask_msg->header,
150  "mono8",
151  mask).toImageMsg());
152 
153  cv::Mat masked_image = image.clone(); // should have the same dtype
154  masked_image.setTo(cval_);
155  image.copyTo(masked_image, mask);
156 
157  cv::Mat output_image;
159  if (sensor_msgs::image_encodings::isMono(image_msg->encoding)) {
160  cv::cvtColor(masked_image, output_image, CV_GRAY2BGRA);
161  }
162  else if (jsk_recognition_utils::isRGB(image_msg->encoding)) {
163  cv::cvtColor(masked_image, output_image, CV_RGB2BGRA);
164  }
165  else { // BGR, BGRA or RGBA
166  cv::cvtColor(masked_image, output_image, CV_BGR2BGRA);
167  }
168  for (size_t j=0; j < mask.rows; j++) {
169  for (int i=0; i < mask.cols; i++) {
170  if (mask.at<uchar>(j, i) == 0) {
171  cv::Vec4b color = output_image.at<cv::Vec4b>(j, i);
172  color[3] = 0; // mask black -> transparent
173  output_image.at<cv::Vec4b>(j, i) = color;
174  }
175  }
176  }
177  // publish bgr8 image
179  image_msg->header,
181  output_image).toImageMsg());
182  }
183  else {
184  if (jsk_recognition_utils::isBGRA(image_msg->encoding)) {
185  cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2BGRA);
186  }
187  else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) {
188  cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2RGBA);
189  }
190  else { // BGR, RGB or GRAY
191  masked_image.copyTo(output_image);
192  }
194  image_msg->header,
195  image_msg->encoding,
196  output_image).toImageMsg());
197  }
198  }
199 }
200 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
bool isRGBA(const std::string &encoding)
#define NODELET_ERROR(...)
bool isBGRA(const std::string &encoding)
void publish(const boost::shared_ptr< M > &message) const
virtual void apply(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::Image::ConstPtr &mask_msg)
sensor_msgs::CameraInfo::ConstPtr camera_info_
static bool isMono(const std::string &encoding)
std::vector< std::string > V_string
PLUGINLIB_EXPORT_CLASS(jsk_perception::ApplyMaskImage, nodelet::Nodelet)
boost::shared_ptr< ros::NodeHandle > pnh_
boost::shared_ptr< message_filters::Synchronizer< ApproximateSyncPolicy > > async_
bool warnNoRemap(const std::vector< std::string > names)
bool isRGB(const std::string &encoding)
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
cv::Rect boundingRectOfMaskImage(const cv::Mat &image)
jsk_topic_tools::VitalChecker::Ptr vital_checker_
mutex_t * lock
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)
message_filters::Subscriber< sensor_msgs::Image > sub_image_
message_filters::Subscriber< sensor_msgs::Image > sub_mask_
sensor_msgs::ImagePtr toImageMsg() const
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Mon May 3 2021 03:03:27