crop_decimate.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <boost/version.hpp>
35 #if ((BOOST_VERSION / 100) % 1000) >= 53
36 #include <boost/thread/lock_guard.hpp>
37 #endif
38 
39 #include <ros/ros.h>
40 #include <nodelet/nodelet.h>
43 #include <dynamic_reconfigure/server.h>
44 #include <cv_bridge/cv_bridge.h>
45 #include <image_proc/CropDecimateConfig.h>
46 #include <opencv2/imgproc/imgproc.hpp>
47 
48 namespace image_proc {
49 
50 using namespace cv_bridge; // CvImage, toCvShare
51 
52 class CropDecimateNodelet : public nodelet::Nodelet
53 {
54  // ROS communication
57  int queue_size_;
58  std::string target_frame_id_;
59 
60  boost::mutex connect_mutex_;
62 
63  // Dynamic reconfigure
64  boost::recursive_mutex config_mutex_;
65  typedef image_proc::CropDecimateConfig Config;
66  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
67  boost::shared_ptr<ReconfigureServer> reconfigure_server_;
68  Config config_;
69 
70  virtual void onInit();
71 
72  void connectCb();
73 
74  void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
75  const sensor_msgs::CameraInfoConstPtr& info_msg);
76 
77  void configCb(Config &config, uint32_t level);
78 };
79 
81 {
82  ros::NodeHandle& nh = getNodeHandle();
83  ros::NodeHandle& private_nh = getPrivateNodeHandle();
84  ros::NodeHandle nh_in (nh, "camera");
85  ros::NodeHandle nh_out(nh, "camera_out");
86  it_in_ .reset(new image_transport::ImageTransport(nh_in));
87  it_out_.reset(new image_transport::ImageTransport(nh_out));
88 
89  // Read parameters
90  private_nh.param("queue_size", queue_size_, 5);
91  private_nh.param("target_frame_id", target_frame_id_, std::string());
92 
93  // Set up dynamic reconfigure
94  reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
95  ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2);
96  reconfigure_server_->setCallback(f);
97 
98  // Monitor whether anyone is subscribed to the output
100  ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this);
101  // Make sure we don't enter connectCb() between advertising and assigning to pub_
102  boost::lock_guard<boost::mutex> lock(connect_mutex_);
103  pub_ = it_out_->advertiseCamera("image_raw", 1, connect_cb, connect_cb, connect_cb_info, connect_cb_info);
104 }
105 
106 // Handles (un)subscribing when clients (un)subscribe
108 {
109  boost::lock_guard<boost::mutex> lock(connect_mutex_);
110  if (pub_.getNumSubscribers() == 0)
111  sub_.shutdown();
112  else if (!sub_)
113  {
114  image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
115  sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this, hints);
116  }
117 }
118 
119 template <typename T>
120 void debayer2x2toBGR(const cv::Mat& src, cv::Mat& dst, int R, int G1, int G2, int B)
121 {
122  typedef cv::Vec<T, 3> DstPixel; // 8- or 16-bit BGR
123  dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
124 
125  int src_row_step = src.step1();
126  int dst_row_step = dst.step1();
127  const T* src_row = src.ptr<T>();
128  T* dst_row = dst.ptr<T>();
129 
130  // 2x2 downsample and debayer at once
131  for (int y = 0; y < dst.rows; ++y)
132  {
133  for (int x = 0; x < dst.cols; ++x)
134  {
135  dst_row[x*3 + 0] = src_row[x*2 + B];
136  dst_row[x*3 + 1] = (src_row[x*2 + G1] + src_row[x*2 + G2]) / 2;
137  dst_row[x*3 + 2] = src_row[x*2 + R];
138  }
139  src_row += src_row_step * 2;
140  dst_row += dst_row_step;
141  }
142 }
143 
144 // Templated on pixel size, in bytes (MONO8 = 1, BGR8 = 3, RGBA16 = 8, ...)
145 template <int N>
146 void decimate(const cv::Mat& src, cv::Mat& dst, int decimation_x, int decimation_y)
147 {
148  dst.create(src.rows / decimation_y, src.cols / decimation_x, src.type());
149 
150  int src_row_step = src.step[0] * decimation_y;
151  int src_pixel_step = N * decimation_x;
152  int dst_row_step = dst.step[0];
153 
154  const uint8_t* src_row = src.ptr();
155  uint8_t* dst_row = dst.ptr();
156 
157  for (int y = 0; y < dst.rows; ++y)
158  {
159  const uint8_t* src_pixel = src_row;
160  uint8_t* dst_pixel = dst_row;
161  for (int x = 0; x < dst.cols; ++x)
162  {
163  memcpy(dst_pixel, src_pixel, N); // Should inline with small, fixed N
164  src_pixel += src_pixel_step;
165  dst_pixel += N;
166  }
167  src_row += src_row_step;
168  dst_row += dst_row_step;
169  }
170 }
171 
172 void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
173  const sensor_msgs::CameraInfoConstPtr& info_msg)
174 {
177 
178  Config config;
179  {
180  boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
181  config = config_;
182  }
183  int decimation_x = config.decimation_x;
184  int decimation_y = config.decimation_y;
185 
186  // Compute the ROI we'll actually use
187  bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding);
188  if (is_bayer)
189  {
190  // Odd offsets for Bayer images basically change the Bayer pattern, but that's
191  // unnecessarily complicated to support
192  config.x_offset &= ~0x1;
193  config.y_offset &= ~0x1;
194  config.width &= ~0x1;
195  config.height &= ~0x1;
196  }
197 
198  int max_width = image_msg->width - config.x_offset;
199  if (max_width <= 0)
200  {
201  ROS_WARN_THROTTLE(30., "x offset is outside the input image width: "
202  "%i, x offset: %i.", image_msg->width, config.x_offset);
203  return;
204  }
205  int max_height = image_msg->height - config.y_offset;
206  if (max_height <= 0)
207  {
208  ROS_WARN_THROTTLE(30., "y offset is outside the input image height: "
209  "%i, y offset: %i", image_msg->height, config.y_offset);
210  return;
211  }
212  int width = config.width;
213  int height = config.height;
214  if (width == 0 || width > max_width)
215  width = max_width;
216  if (height == 0 || height > max_height)
217  height = max_height;
218 
219  // On no-op, just pass the messages along
220  if (decimation_x == 1 &&
221  decimation_y == 1 &&
222  config.x_offset == 0 &&
223  config.y_offset == 0 &&
224  width == (int)image_msg->width &&
225  height == (int)image_msg->height)
226  {
227  pub_.publish(image_msg, info_msg);
228  return;
229  }
230 
231  // Get a cv::Mat view of the source data
232  CvImageConstPtr source = toCvShare(image_msg);
233 
234  // Except in Bayer downsampling case, output has same encoding as the input
235  CvImage output(source->header, source->encoding);
236  // Apply ROI (no copy, still a view of the image_msg data)
237  output.image = source->image(cv::Rect(config.x_offset, config.y_offset, width, height));
238 
239  // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR
240  if (is_bayer && (decimation_x > 1 || decimation_y > 1))
241  {
242  if (decimation_x % 2 != 0 || decimation_y % 2 != 0)
243  {
244  NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images");
245  return;
246  }
247 
248  cv::Mat bgr;
249  int step = output.image.step1();
250  if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
251  debayer2x2toBGR<uint8_t>(output.image, bgr, 0, 1, step, step + 1);
252  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
253  debayer2x2toBGR<uint8_t>(output.image, bgr, step + 1, 1, step, 0);
254  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
255  debayer2x2toBGR<uint8_t>(output.image, bgr, step, 0, step + 1, 1);
256  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
257  debayer2x2toBGR<uint8_t>(output.image, bgr, 1, 0, step + 1, step);
258  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16)
259  debayer2x2toBGR<uint16_t>(output.image, bgr, 0, 1, step, step + 1);
260  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16)
261  debayer2x2toBGR<uint16_t>(output.image, bgr, step + 1, 1, step, 0);
262  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16)
263  debayer2x2toBGR<uint16_t>(output.image, bgr, step, 0, step + 1, 1);
264  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16)
265  debayer2x2toBGR<uint16_t>(output.image, bgr, 1, 0, step + 1, step);
266  else
267  {
268  NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
269  return;
270  }
271 
272  output.image = bgr;
273  output.encoding = (bgr.depth() == CV_8U) ? sensor_msgs::image_encodings::BGR8
275  decimation_x /= 2;
276  decimation_y /= 2;
277  }
278 
279  // Apply further downsampling, if necessary
280  if (decimation_x > 1 || decimation_y > 1)
281  {
282  cv::Mat decimated;
283 
284  if (config.interpolation == image_proc::CropDecimate_NN)
285  {
286  // Use optimized method instead of OpenCV's more general NN resize
287  int pixel_size = output.image.elemSize();
288  switch (pixel_size)
289  {
290  // Currently support up through 4-channel float
291  case 1:
292  decimate<1>(output.image, decimated, decimation_x, decimation_y);
293  break;
294  case 2:
295  decimate<2>(output.image, decimated, decimation_x, decimation_y);
296  break;
297  case 3:
298  decimate<3>(output.image, decimated, decimation_x, decimation_y);
299  break;
300  case 4:
301  decimate<4>(output.image, decimated, decimation_x, decimation_y);
302  break;
303  case 6:
304  decimate<6>(output.image, decimated, decimation_x, decimation_y);
305  break;
306  case 8:
307  decimate<8>(output.image, decimated, decimation_x, decimation_y);
308  break;
309  case 12:
310  decimate<12>(output.image, decimated, decimation_x, decimation_y);
311  break;
312  case 16:
313  decimate<16>(output.image, decimated, decimation_x, decimation_y);
314  break;
315  default:
316  NODELET_ERROR_THROTTLE(2, "Unsupported pixel size, %d bytes", pixel_size);
317  return;
318  }
319  }
320  else
321  {
322  // Linear, cubic, area, ...
323  cv::Size size(output.image.cols / decimation_x, output.image.rows / decimation_y);
324  cv::resize(output.image, decimated, size, 0.0, 0.0, config.interpolation);
325  }
326 
327  output.image = decimated;
328  }
329 
330  // Create output Image message
332  sensor_msgs::ImagePtr out_image = output.toImageMsg();
333 
334  // Create updated CameraInfo message
335  sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
336  int binning_x = std::max((int)info_msg->binning_x, 1);
337  int binning_y = std::max((int)info_msg->binning_y, 1);
338  out_info->binning_x = binning_x * config.decimation_x;
339  out_info->binning_y = binning_y * config.decimation_y;
340  out_info->roi.x_offset += config.x_offset * binning_x;
341  out_info->roi.y_offset += config.y_offset * binning_y;
342  out_info->roi.height = height * binning_y;
343  out_info->roi.width = width * binning_x;
344  // If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true.
345  if (width != (int)image_msg->width || height != (int)image_msg->height)
346  out_info->roi.do_rectify = true;
347 
348  if (!target_frame_id_.empty()) {
349  out_image->header.frame_id = target_frame_id_;
350  out_info->header.frame_id = target_frame_id_;
351  }
352 
353  pub_.publish(out_image, out_info);
354 }
355 
356 void CropDecimateNodelet::configCb(Config &config, uint32_t level)
357 {
358  config_ = config;
359 }
360 
361 } // namespace image_proc
362 
363 // Register nodelet
sensor_msgs::image_encodings::BAYER_RGGB8
const std::string BAYER_RGGB8
image_encodings.h
image_proc::CropDecimateNodelet::onInit
virtual void onInit()
Definition: crop_decimate.cpp:112
image_transport::ImageTransport
image_proc::debayer2x2toBGR
void debayer2x2toBGR(const cv::Mat &src, cv::Mat &dst, int R, int G1, int G2, int B)
Definition: crop_decimate.cpp:152
boost::shared_ptr< image_transport::ImageTransport >
NODELET_ERROR_THROTTLE
#define NODELET_ERROR_THROTTLE(rate,...)
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(period,...)
ros.h
ros::TransportHints
sensor_msgs::image_encodings::BAYER_GBRG8
const std::string BAYER_GBRG8
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
sensor_msgs::image_encodings::isBayer
static bool isBayer(const std::string &encoding)
f
f
image_proc::decimate
void decimate(const cv::Mat &src, cv::Mat &dst, int decimation_x, int decimation_y)
Definition: crop_decimate.cpp:178
image_proc::CropDecimateNodelet::connectCb
void connectCb()
Definition: crop_decimate.cpp:139
class_list_macros.h
sensor_msgs::image_encodings::BGR16
const std::string BGR16
image_proc::CropDecimateNodelet::ReconfigureServer
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: crop_decimate.cpp:98
image_transport::CameraSubscriber
image_proc::CropDecimateNodelet::configCb
void configCb(Config &config, uint32_t level)
Definition: crop_decimate.cpp:388
sensor_msgs::image_encodings::BAYER_BGGR8
const std::string BAYER_BGGR8
image_transport::CameraPublisher
sensor_msgs::image_encodings::BAYER_BGGR16
const std::string BAYER_BGGR16
sensor_msgs::image_encodings::BAYER_GBRG16
const std::string BAYER_GBRG16
image_transport.h
nodelet::Nodelet
image_proc
Definition: advertisement_checker.h:39
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
nodelet.h
cv_bridge.h
cv_bridge::CvImage
image_proc::CropDecimateNodelet::imageCb
void imageCb(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
Definition: crop_decimate.cpp:204
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
image_transport::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
sensor_msgs::image_encodings::BGR8
const std::string BGR8
sensor_msgs::image_encodings::BAYER_GRBG8
const std::string BAYER_GRBG8
image_transport::TransportHints
cv_bridge
image_proc::CropDecimateNodelet
Definition: crop_decimate.cpp:84
toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
sensor_msgs::image_encodings::BAYER_GRBG16
const std::string BAYER_GRBG16
ros::NodeHandle
sensor_msgs::image_encodings::BAYER_RGGB16
const std::string BAYER_RGGB16


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Mon Nov 15 2021 03:57:55