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 
53 {
54  // ROS communication
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;
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  int max_height = image_msg->height - config.y_offset;
200  int width = config.width;
201  int height = config.height;
202  if (width == 0 || width > max_width)
203  width = max_width;
204  if (height == 0 || height > max_height)
205  height = max_height;
206 
207  // On no-op, just pass the messages along
208  if (decimation_x == 1 &&
209  decimation_y == 1 &&
210  config.x_offset == 0 &&
211  config.y_offset == 0 &&
212  width == (int)image_msg->width &&
213  height == (int)image_msg->height)
214  {
215  pub_.publish(image_msg, info_msg);
216  return;
217  }
218 
219  // Get a cv::Mat view of the source data
220  CvImageConstPtr source = toCvShare(image_msg);
221 
222  // Except in Bayer downsampling case, output has same encoding as the input
223  CvImage output(source->header, source->encoding);
224  // Apply ROI (no copy, still a view of the image_msg data)
225  output.image = source->image(cv::Rect(config.x_offset, config.y_offset, width, height));
226 
227  // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR
228  if (is_bayer && (decimation_x > 1 || decimation_y > 1))
229  {
230  if (decimation_x % 2 != 0 || decimation_y % 2 != 0)
231  {
232  NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images");
233  return;
234  }
235 
236  cv::Mat bgr;
237  int step = output.image.step1();
238  if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
239  debayer2x2toBGR<uint8_t>(output.image, bgr, 0, 1, step, step + 1);
240  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
241  debayer2x2toBGR<uint8_t>(output.image, bgr, step + 1, 1, step, 0);
242  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
243  debayer2x2toBGR<uint8_t>(output.image, bgr, step, 0, step + 1, 1);
244  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
245  debayer2x2toBGR<uint8_t>(output.image, bgr, 1, 0, step + 1, step);
246  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16)
247  debayer2x2toBGR<uint16_t>(output.image, bgr, 0, 1, step, step + 1);
248  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16)
249  debayer2x2toBGR<uint16_t>(output.image, bgr, step + 1, 1, step, 0);
250  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16)
251  debayer2x2toBGR<uint16_t>(output.image, bgr, step, 0, step + 1, 1);
252  else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16)
253  debayer2x2toBGR<uint16_t>(output.image, bgr, 1, 0, step + 1, step);
254  else
255  {
256  NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
257  return;
258  }
259 
260  output.image = bgr;
261  output.encoding = (bgr.depth() == CV_8U) ? sensor_msgs::image_encodings::BGR8
263  decimation_x /= 2;
264  decimation_y /= 2;
265  }
266 
267  // Apply further downsampling, if necessary
268  if (decimation_x > 1 || decimation_y > 1)
269  {
270  cv::Mat decimated;
271 
272  if (config.interpolation == image_proc::CropDecimate_NN)
273  {
274  // Use optimized method instead of OpenCV's more general NN resize
275  int pixel_size = output.image.elemSize();
276  switch (pixel_size)
277  {
278  // Currently support up through 4-channel float
279  case 1:
280  decimate<1>(output.image, decimated, decimation_x, decimation_y);
281  break;
282  case 2:
283  decimate<2>(output.image, decimated, decimation_x, decimation_y);
284  break;
285  case 3:
286  decimate<3>(output.image, decimated, decimation_x, decimation_y);
287  break;
288  case 4:
289  decimate<4>(output.image, decimated, decimation_x, decimation_y);
290  break;
291  case 6:
292  decimate<6>(output.image, decimated, decimation_x, decimation_y);
293  break;
294  case 8:
295  decimate<8>(output.image, decimated, decimation_x, decimation_y);
296  break;
297  case 12:
298  decimate<12>(output.image, decimated, decimation_x, decimation_y);
299  break;
300  case 16:
301  decimate<16>(output.image, decimated, decimation_x, decimation_y);
302  break;
303  default:
304  NODELET_ERROR_THROTTLE(2, "Unsupported pixel size, %d bytes", pixel_size);
305  return;
306  }
307  }
308  else
309  {
310  // Linear, cubic, area, ...
311  cv::Size size(output.image.cols / decimation_x, output.image.rows / decimation_y);
312  cv::resize(output.image, decimated, size, 0.0, 0.0, config.interpolation);
313  }
314 
315  output.image = decimated;
316  }
317 
318  // Create output Image message
320  sensor_msgs::ImagePtr out_image = output.toImageMsg();
321 
322  // Create updated CameraInfo message
323  sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
324  int binning_x = std::max((int)info_msg->binning_x, 1);
325  int binning_y = std::max((int)info_msg->binning_y, 1);
326  out_info->binning_x = binning_x * config.decimation_x;
327  out_info->binning_y = binning_y * config.decimation_y;
328  out_info->roi.x_offset += config.x_offset * binning_x;
329  out_info->roi.y_offset += config.y_offset * binning_y;
330  out_info->roi.height = height * binning_y;
331  out_info->roi.width = width * binning_x;
332  // If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true.
333  if (width != (int)image_msg->width || height != (int)image_msg->height)
334  out_info->roi.do_rectify = true;
335 
336  if (!target_frame_id_.empty()) {
337  out_image->header.frame_id = target_frame_id_;
338  out_info->header.frame_id = target_frame_id_;
339  }
340 
341  pub_.publish(out_image, out_info);
342 }
343 
344 void CropDecimateNodelet::configCb(Config &config, uint32_t level)
345 {
346  config_ = config;
347 }
348 
349 } // namespace image_proc
350 
351 // Register nodelet
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
const std::string BAYER_GRBG8
const std::string BAYER_GRBG16
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
f
const std::string BAYER_RGGB16
static bool isBayer(const std::string &encoding)
#define NODELET_ERROR_THROTTLE(rate,...)
void debayer2x2toBGR(const cv::Mat &src, cv::Mat &dst, int R, int G1, int G2, int B)
std::string encoding
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void decimate(const cv::Mat &src, cv::Mat &dst, int decimation_x, int decimation_y)
void configCb(Config &config, uint32_t level)
const std::string BAYER_GBRG8
const std::string BAYER_GBRG16
boost::shared_ptr< image_transport::ImageTransport > it_out_
const std::string BAYER_BGGR16
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
image_proc::CropDecimateConfig Config
boost::recursive_mutex config_mutex_
image_transport::CameraSubscriber sub_
dynamic_reconfigure::Server< Config > ReconfigureServer
const std::string BAYER_BGGR8
boost::shared_ptr< ReconfigureServer > reconfigure_server_
void imageCb(const sensor_msgs::ImageConstPtr &image_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
const std::string BAYER_RGGB8
image_transport::CameraPublisher pub_
sensor_msgs::ImagePtr toImageMsg() const


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Thu Nov 7 2019 03:44:58