debayer.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/make_shared.hpp>
35 #include <boost/version.hpp>
36 #if ((BOOST_VERSION / 100) % 1000) >= 53
37 #include <boost/thread/lock_guard.hpp>
38 #endif
39 
40 #include <ros/ros.h>
41 #include <nodelet/nodelet.h>
44 #include <dynamic_reconfigure/server.h>
45 #include <image_proc/DebayerConfig.h>
46 
47 #include <opencv2/imgproc/imgproc.hpp>
48 // Until merged into OpenCV
49 #include "edge_aware.h"
50 
51 #include <cv_bridge/cv_bridge.h>
52 
53 namespace image_proc {
54 
56 
58 {
59  // ROS communication
62 
63  boost::mutex connect_mutex_;
66 
67  // Dynamic reconfigure
68  boost::recursive_mutex config_mutex_;
69  typedef image_proc::DebayerConfig Config;
70  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
72  Config config_;
73 
74  virtual void onInit();
75 
76  void connectCb();
77 
78  void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
79 
80  void configCb(Config &config, uint32_t level);
81 };
82 
84 {
86  ros::NodeHandle &private_nh = getPrivateNodeHandle();
87  it_.reset(new image_transport::ImageTransport(nh));
88 
89  // Set up dynamic reconfigure
90  reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
91  ReconfigureServer::CallbackType f = boost::bind(&DebayerNodelet::configCb, this, _1, _2);
92  reconfigure_server_->setCallback(f);
93 
94  // Monitor whether anyone is subscribed to the output
96  ConnectCB connect_cb = boost::bind(&DebayerNodelet::connectCb, this);
97  // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX
98  boost::lock_guard<boost::mutex> lock(connect_mutex_);
99  pub_mono_ = it_->advertise("image_mono", 1, connect_cb, connect_cb);
100  pub_color_ = it_->advertise("image_color", 1, connect_cb, connect_cb);
101 }
102 
103 // Handles (un)subscribing when clients (un)subscribe
105 {
106  boost::lock_guard<boost::mutex> lock(connect_mutex_);
108  sub_raw_.shutdown();
109  else if (!sub_raw_)
110  {
112  sub_raw_ = it_->subscribe("image_raw", 1, &DebayerNodelet::imageCb, this, hints);
113  }
114 }
115 
116 void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
117 {
118  int bit_depth = enc::bitDepth(raw_msg->encoding);
119  //@todo Fix as soon as bitDepth fixes it
120  if (raw_msg->encoding == enc::YUV422)
121  bit_depth = 8;
122 
123  // First publish to mono if needed
125  {
126  if (enc::isMono(raw_msg->encoding))
127  pub_mono_.publish(raw_msg);
128  else
129  {
130  if ((bit_depth != 8) && (bit_depth != 16))
131  {
133  "Raw image data from topic '%s' has unsupported depth: %d",
134  sub_raw_.getTopic().c_str(), bit_depth);
135  } else {
136  // Use cv_bridge to convert to Mono. If a type is not supported,
137  // it will error out there
138  sensor_msgs::ImagePtr gray_msg;
139  try
140  {
141  if (bit_depth == 8)
142  gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO8)->toImageMsg();
143  else
144  gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO16)->toImageMsg();
145  pub_mono_.publish(gray_msg);
146  }
147  catch (cv_bridge::Exception &e)
148  {
149  NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
150  }
151  }
152  }
153  }
154 
155  // Next, publish to color
157  return;
158 
159  if (enc::isMono(raw_msg->encoding))
160  {
161  // For monochrome, no processing needed!
162  pub_color_.publish(raw_msg);
163 
164  // Warn if the user asked for color
166  "Color topic '%s' requested, but raw image data from topic '%s' is grayscale",
167  pub_color_.getTopic().c_str(), sub_raw_.getTopic().c_str());
168  }
169  else if (enc::isColor(raw_msg->encoding))
170  {
171  pub_color_.publish(raw_msg);
172  }
173  else if (enc::isBayer(raw_msg->encoding)) {
174  int type = bit_depth == 8 ? CV_8U : CV_16U;
175  const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1),
176  const_cast<uint8_t*>(&raw_msg->data[0]), raw_msg->step);
177 
178  sensor_msgs::ImagePtr color_msg = boost::make_shared<sensor_msgs::Image>();
179  color_msg->header = raw_msg->header;
180  color_msg->height = raw_msg->height;
181  color_msg->width = raw_msg->width;
182  color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16;
183  color_msg->step = color_msg->width * 3 * (bit_depth / 8);
184  color_msg->data.resize(color_msg->height * color_msg->step);
185 
186  cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3),
187  &color_msg->data[0], color_msg->step);
188 
189  int algorithm;
190  {
191  boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
192  algorithm = config_.debayer;
193  }
194 
195  if (algorithm == Debayer_EdgeAware ||
196  algorithm == Debayer_EdgeAwareWeighted)
197  {
198  // These algorithms are not in OpenCV yet
199  if (raw_msg->encoding != enc::BAYER_GRBG8)
200  {
201  NODELET_WARN_THROTTLE(30, "Edge aware algorithms currently only support GRBG8 Bayer. "
202  "Falling back to bilinear interpolation.");
203  algorithm = Debayer_Bilinear;
204  }
205  else
206  {
207  if (algorithm == Debayer_EdgeAware)
208  debayerEdgeAware(bayer, color);
209  else
210  debayerEdgeAwareWeighted(bayer, color);
211  }
212  }
213  if (algorithm == Debayer_Bilinear ||
214  algorithm == Debayer_VNG)
215  {
216  int code = -1;
217  if (raw_msg->encoding == enc::BAYER_RGGB8 ||
218  raw_msg->encoding == enc::BAYER_RGGB16)
219  code = cv::COLOR_BayerBG2BGR;
220  else if (raw_msg->encoding == enc::BAYER_BGGR8 ||
221  raw_msg->encoding == enc::BAYER_BGGR16)
222  code = cv::COLOR_BayerRG2BGR;
223  else if (raw_msg->encoding == enc::BAYER_GBRG8 ||
224  raw_msg->encoding == enc::BAYER_GBRG16)
225  code = cv::COLOR_BayerGR2BGR;
226  else if (raw_msg->encoding == enc::BAYER_GRBG8 ||
227  raw_msg->encoding == enc::BAYER_GRBG16)
228  code = cv::COLOR_BayerGB2BGR;
229 
230  if (algorithm == Debayer_VNG)
231  code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR;
232 
233  cv::cvtColor(bayer, color, code);
234  }
235 
236  pub_color_.publish(color_msg);
237  }
238  else if (raw_msg->encoding == enc::YUV422)
239  {
240  // Use cv_bridge to convert to BGR8
241  sensor_msgs::ImagePtr color_msg;
242  try
243  {
244  color_msg = cv_bridge::toCvCopy(raw_msg, enc::BGR8)->toImageMsg();
245  pub_color_.publish(color_msg);
246  }
247  catch (cv_bridge::Exception &e)
248  {
249  NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
250  }
251  }
252  else if (raw_msg->encoding == enc::TYPE_8UC3)
253  {
254  // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...?
256  "Raw image topic '%s' has ambiguous encoding '8UC3'. The "
257  "source should set the encoding to 'bgr8' or 'rgb8'.",
258  sub_raw_.getTopic().c_str());
259  }
260  else
261  {
262  NODELET_ERROR_THROTTLE(10, "Raw image topic '%s' has unsupported encoding '%s'",
263  sub_raw_.getTopic().c_str(), raw_msg->encoding.c_str());
264  }
265 }
266 
267 void DebayerNodelet::configCb(Config &config, uint32_t level)
268 {
269  config_ = config;
270 }
271 
272 } // namespace image_proc
273 
274 // Register nodelet
image_transport::Publisher pub_color_
Definition: debayer.cpp:65
f
boost::recursive_mutex config_mutex_
Definition: debayer.cpp:68
boost::mutex connect_mutex_
Definition: debayer.cpp:63
#define NODELET_ERROR_THROTTLE(rate,...)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
image_transport::Publisher pub_mono_
Definition: debayer.cpp:64
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
std::string getTopic() const
boost::shared_ptr< image_transport::ImageTransport > it_
Definition: debayer.cpp:60
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Definition: debayer.cpp:71
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
void publish(const sensor_msgs::Image &message) const
#define NODELET_WARN_THROTTLE(rate,...)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
virtual void onInit()
Definition: debayer.cpp:83
ros::NodeHandle & getNodeHandle() const
void imageCb(const sensor_msgs::ImageConstPtr &raw_msg)
Definition: debayer.cpp:116
void configCb(Config &config, uint32_t level)
Definition: debayer.cpp:267
void debayerEdgeAwareWeighted(const cv::Mat &bayer, cv::Mat &color)
Definition: edge_aware.cpp:426
void debayerEdgeAware(const cv::Mat &bayer, cv::Mat &color)
Definition: edge_aware.cpp:44
image_proc::DebayerConfig Config
Definition: debayer.cpp:69
image_transport::Subscriber sub_raw_
Definition: debayer.cpp:61
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: debayer.cpp:70
std::string getTopic() const


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Fri May 3 2019 02:59:50