draw_rects.cpp
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017, 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 /*
36  * draw_rects.cpp
37  * Author: furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
38  */
39 
41 #if ( CV_MAJOR_VERSION >= 4)
42 #include <opencv2/imgproc/imgproc_c.h>
43 #endif
44 
46 
47 namespace jsk_perception
48 {
49 
51  {
52  ConnectionBasedNodelet::onInit();
53 
54  srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
55  typename dynamic_reconfigure::Server<Config>::CallbackType f =
56  boost::bind(&DrawRects::configCallback, this, _1, _2);
57  srv_->setCallback(f);
58 
59  pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
60 
62  }
63 
65  {
66  sub_image_.subscribe(*pnh_, "input", 1);
67  sub_rects_.subscribe(*pnh_, "input/rects", 1);
68 
70  sub_class_.subscribe(*pnh_, "input/class", 1);
71  else
73 
74  if (use_async_)
75  {
76  async_ = boost::make_shared<message_filters::Synchronizer<AsyncPolicy> >(queue_size_);
78  async_->connectInput(sub_image_, sub_rects_, sub_class_);
79  else
80  async_->connectInput(sub_image_, sub_rects_, null_class_);
81  async_->registerCallback(boost::bind(&DrawRects::onMessage, this, _1, _2, _3));
82  } else {
83  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
85  sync_->connectInput(sub_image_, sub_rects_, sub_class_);
86  else
87  sync_->connectInput(sub_image_, sub_rects_, null_class_);
88  sync_->registerCallback(boost::bind(&DrawRects::onMessage, this, _1, _2, _3));
89  }
90  }
91 
93  {
98  }
99 
100  void DrawRects::configCallback(Config& config, uint32_t level)
101  {
102  boost::mutex::scoped_lock lock(mutex_);
103 
104  bool need_resubscribe = false;
105  if (use_async_ != config.approximate_sync ||
106  queue_size_ != config.queue_size ||
107  use_classification_result_ != config.use_classification_result)
108  need_resubscribe = true;
109 
110  use_async_ = config.approximate_sync;
111  queue_size_ = config.queue_size;
112 
113  use_classification_result_ = config.use_classification_result;
114  show_proba_ = config.show_proba;
115 
116  rect_boldness_ = config.rect_boldness;
117 
118  label_size_ = config.label_size;
119  label_boldness_ = config.label_boldness;
120  label_font_ = config.label_font;
121  label_margin_factor_ = config.label_margin_factor;
122 
123  resolution_factor_ = config.resolution_factor;
124  interpolation_method_ = config.interpolation_method;
125 #if ROS_VERSION_MINIMUM(1, 11, 4)
126  // indigo or later
127  if (need_resubscribe && isSubscribed()) {
128  unsubscribe();
129  subscribe();
130  }
131 #endif
132  }
133 
135  const jsk_recognition_msgs::RectArray::ConstPtr& rects)
136  {
137  jsk_recognition_msgs::ClassificationResult classes;
138  classes.header = rects->header;
140  boost::make_shared<jsk_recognition_msgs::ClassificationResult>(classes));
141  }
142 
144  const sensor_msgs::Image::ConstPtr& image,
145  const jsk_recognition_msgs::RectArray::ConstPtr& rects,
146  const jsk_recognition_msgs::ClassificationResult::ConstPtr& classes)
147  {
148  boost::mutex::scoped_lock lock(mutex_);
149 
151  try {
152  cv_img = cv_bridge::toCvCopy(image, enc::BGR8);
153  } catch (cv_bridge::Exception &e) {
154  NODELET_ERROR_STREAM("Failed to convert image: " << e.what());
155  return;
156  }
157 
158  cv::Mat img;
159  cv::resize(cv_img->image, img, cv::Size(),
162 
163  int label_num = \
164  use_classification_result_ ? classes->target_names.size() : rects->rects.size();
165 
166  for (size_t i = 0; i < rects->rects.size(); ++i) {
167  int label_idx = use_classification_result_ ? classes->labels[i] : i;
168  cv::Scalar color;
169  randomColor(label_num, label_idx, color);
170  drawRect(img, rects->rects[i], color);
171 
173  {
174  std::ostringstream oss;
175  oss << classes->label_names[i];
176  if (show_proba_ && classes->label_proba.size() > i) {
177  oss << std::fixed << std::setprecision(2);
178  oss << " (" << classes->label_proba[i] << ")";
179  }
180  drawLabel(img, rects->rects[i], color, oss.str());
181  }
182  }
183 
184  pub_image_.publish(*cv_bridge::CvImage(image->header, enc::BGR8, img).toImageMsg());
185  }
186 
188  cv::Mat& img, const jsk_recognition_msgs::Rect& orig_rect, const cv::Scalar& color)
189  {
190  cv::Rect rect(orig_rect.x * resolution_factor_,
191  orig_rect.y * resolution_factor_,
192  orig_rect.width * resolution_factor_,
193  orig_rect.height * resolution_factor_);
194  cv::rectangle(img, rect, color, rect_boldness_, CV_AA);
195  }
196 
198  cv::Mat& img, const jsk_recognition_msgs::Rect& rect,
199  const cv::Scalar& color, const std::string& label)
200  {
201  int baseline;
202  cv::Size label_size = cv::getTextSize(label, label_font_,
204  &baseline);
205  int text_color = isDarkColor(color) ? 255 : 0;
206 
207  double orig_x = rect.x * resolution_factor_;
208  double orig_y = rect.y * resolution_factor_;
209 
210  cv::rectangle(img,
211  cv::Rect(orig_x,
212  orig_y - label_size.height * label_margin_factor_ * 1.15,
213  label_size.width * label_margin_factor_,
214  label_size.height * label_margin_factor_ * 1.3),
215  color, -1, CV_AA);
216  cv::putText(img, label,
217  cv::Point(orig_x + label_size.width * (label_margin_factor_ - 1.0) / 2.0,
218  orig_y - label_size.height * (label_margin_factor_ - 1.0) / 2.0),
219  label_font_,
220  label_size_,
221  cv::Scalar(text_color, text_color, text_color),
223  CV_AA);
224  }
225 
226  void DrawRects::randomColor(const int& label_num, const int& index, cv::Scalar& color)
227  {
228  static const float colors[6][3] = { {1,0,1}, {0,0,1},{0,1,1},{0,1,0},{1,1,0},{1,0,0} };
229  float ratio = ((float)(index * 123457 % label_num) / label_num) * 5;
230  int i = std::floor(ratio);
231  int j = std::ceil(ratio);
232  ratio -= i;
233  for (int c = 0; c < 3; ++c)
234  color[c] = (int)(((1-ratio) * colors[i][c] + ratio * colors[j][c]) * 255);
235  }
236 }
237 
f
virtual void drawRect(cv::Mat &img, const jsk_recognition_msgs::Rect &rect, const cv::Scalar &color)
Definition: draw_rects.cpp:187
void publish(const boost::shared_ptr< M > &message) const
virtual void fillEmptyClasses(const jsk_recognition_msgs::RectArray::ConstPtr &rects)
Definition: draw_rects.cpp:134
virtual void configCallback(Config &config, uint32_t level)
Definition: draw_rects.cpp:100
virtual void subscribe()
Definition: draw_rects.cpp:64
message_filters::Subscriber< jsk_recognition_msgs::ClassificationResult > sub_class_
Definition: draw_rects.h:112
float
#define NODELET_ERROR_STREAM(...)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
Definition: draw_rects.h:106
message_filters::Subscriber< jsk_recognition_msgs::RectArray > sub_rects_
Definition: draw_rects.h:111
virtual void randomColor(const int &label_num, const int &index, cv::Scalar &color)
Definition: draw_rects.cpp:226
virtual void unsubscribe()
Definition: draw_rects.cpp:92
boost::shared_ptr< ros::NodeHandle > pnh_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: draw_rects.h:105
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void onMessage(const sensor_msgs::Image::ConstPtr &image, const jsk_recognition_msgs::RectArray::ConstPtr &rects, const jsk_recognition_msgs::ClassificationResult::ConstPtr &classes)
Definition: draw_rects.cpp:143
message_filters::Subscriber< sensor_msgs::Image > sub_image_
Definition: draw_rects.h:110
message_filters::PassThrough< jsk_recognition_msgs::ClassificationResult > null_class_
Definition: draw_rects.h:109
DrawRectsConfig Config
Definition: draw_rects.h:72
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)
void add(const MConstPtr &msg)
boost::shared_ptr< message_filters::Synchronizer< AsyncPolicy > > async_
Definition: draw_rects.h:107
c
PLUGINLIB_EXPORT_CLASS(jsk_perception::DrawRects, nodelet::Nodelet)
GdkColor colors[]
ros::Publisher pub_image_
Definition: draw_rects.h:108
virtual bool isDarkColor(const cv::Scalar &color)
Definition: draw_rects.h:100
sensor_msgs::ImagePtr toImageMsg() const
virtual void drawLabel(cv::Mat &img, const jsk_recognition_msgs::Rect &rect, const cv::Scalar &color, const std::string &label)
Definition: draw_rects.cpp:197
Connection registerCallback(const C &callback)


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