debayer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <boost/make_shared.hpp>
00035 #include <boost/version.hpp>
00036 #if ((BOOST_VERSION / 100) % 1000) >= 53
00037 #include <boost/thread/lock_guard.hpp>
00038 #endif
00039 
00040 #include <ros/ros.h>
00041 #include <nodelet/nodelet.h>
00042 #include <image_transport/image_transport.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <dynamic_reconfigure/server.h>
00045 #include <image_proc/DebayerConfig.h>
00046 
00047 #include <opencv2/imgproc/imgproc.hpp>
00048 // Until merged into OpenCV
00049 #include "edge_aware.h"
00050 
00051 #include <cv_bridge/cv_bridge.h>
00052 
00053 namespace image_proc {
00054 
00055 namespace enc = sensor_msgs::image_encodings;
00056 
00057 class DebayerNodelet : public nodelet::Nodelet
00058 {
00059   // ROS communication
00060   boost::shared_ptr<image_transport::ImageTransport> it_;
00061   image_transport::Subscriber sub_raw_;
00062   
00063   boost::mutex connect_mutex_;
00064   image_transport::Publisher pub_mono_;
00065   image_transport::Publisher pub_color_;
00066 
00067   // Dynamic reconfigure
00068   boost::recursive_mutex config_mutex_;
00069   typedef image_proc::DebayerConfig Config;
00070   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00071   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00072   Config config_;
00073 
00074   virtual void onInit();
00075 
00076   void connectCb();
00077 
00078   void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
00079 
00080   void configCb(Config &config, uint32_t level);
00081 };
00082 
00083 void DebayerNodelet::onInit()
00084 {
00085   ros::NodeHandle &nh         = getNodeHandle();
00086   ros::NodeHandle &private_nh = getPrivateNodeHandle();
00087   it_.reset(new image_transport::ImageTransport(nh));
00088 
00089   // Set up dynamic reconfigure
00090   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
00091   ReconfigureServer::CallbackType f = boost::bind(&DebayerNodelet::configCb, this, _1, _2);
00092   reconfigure_server_->setCallback(f);
00093 
00094   // Monitor whether anyone is subscribed to the output
00095   typedef image_transport::SubscriberStatusCallback ConnectCB;
00096   ConnectCB connect_cb = boost::bind(&DebayerNodelet::connectCb, this);
00097   // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX
00098   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00099   pub_mono_  = it_->advertise("image_mono",  1, connect_cb, connect_cb);
00100   pub_color_ = it_->advertise("image_color", 1, connect_cb, connect_cb);
00101 }
00102 
00103 // Handles (un)subscribing when clients (un)subscribe
00104 void DebayerNodelet::connectCb()
00105 {
00106   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00107   if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0)
00108     sub_raw_.shutdown();
00109   else if (!sub_raw_)
00110   {
00111     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00112     sub_raw_ = it_->subscribe("image_raw", 1, &DebayerNodelet::imageCb, this, hints);
00113   }
00114 }
00115 
00116 void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
00117 {
00118   int bit_depth = enc::bitDepth(raw_msg->encoding);
00119   //@todo Fix as soon as bitDepth fixes it
00120   if (raw_msg->encoding == enc::YUV422)
00121     bit_depth = 8;
00122 
00123   // First publish to mono if needed
00124   if (pub_mono_.getNumSubscribers())
00125   {
00126     if (enc::isMono(raw_msg->encoding))
00127       pub_mono_.publish(raw_msg);
00128     else
00129     {
00130       if ((bit_depth != 8) && (bit_depth != 16))
00131       {
00132         NODELET_WARN_THROTTLE(30,
00133                             "Raw image data from topic '%s' has unsupported depth: %d",
00134                             sub_raw_.getTopic().c_str(), bit_depth);
00135       } else {
00136         // Use cv_bridge to convert to Mono. If a type is not supported,
00137         // it will error out there
00138         sensor_msgs::ImagePtr gray_msg;
00139         try
00140         {
00141           if (bit_depth == 8)
00142             gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO8)->toImageMsg();
00143           else
00144             gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO16)->toImageMsg();
00145           pub_mono_.publish(gray_msg);
00146         }
00147         catch (cv_bridge::Exception &e)
00148         {
00149           NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
00150         }
00151       }
00152     }
00153   }
00154 
00155   // Next, publish to color
00156   if (!pub_color_.getNumSubscribers())
00157     return;
00158 
00159   if (enc::isMono(raw_msg->encoding))
00160   {
00161     // For monochrome, no processing needed!
00162     pub_color_.publish(raw_msg);
00163 
00164     // Warn if the user asked for color
00165     NODELET_WARN_THROTTLE(30,
00166                             "Color topic '%s' requested, but raw image data from topic '%s' is grayscale",
00167                             pub_color_.getTopic().c_str(), sub_raw_.getTopic().c_str());
00168   }
00169   else if (enc::isColor(raw_msg->encoding))
00170   {
00171     pub_color_.publish(raw_msg);
00172   }
00173   else if (enc::isBayer(raw_msg->encoding)) {
00174     int type = bit_depth == 8 ? CV_8U : CV_16U;
00175     const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1),
00176                         const_cast<uint8_t*>(&raw_msg->data[0]), raw_msg->step);
00177 
00178       sensor_msgs::ImagePtr color_msg = boost::make_shared<sensor_msgs::Image>();
00179       color_msg->header   = raw_msg->header;
00180       color_msg->height   = raw_msg->height;
00181       color_msg->width    = raw_msg->width;
00182       color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16;
00183       color_msg->step     = color_msg->width * 3 * (bit_depth / 8);
00184       color_msg->data.resize(color_msg->height * color_msg->step);
00185 
00186       cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3),
00187                     &color_msg->data[0], color_msg->step);
00188 
00189       int algorithm;
00190       {
00191         boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00192         algorithm = config_.debayer;
00193       }
00194       
00195       if (algorithm == Debayer_EdgeAware ||
00196           algorithm == Debayer_EdgeAwareWeighted)
00197       {
00198         // These algorithms are not in OpenCV yet
00199         if (raw_msg->encoding != enc::BAYER_GRBG8)
00200         {
00201           NODELET_WARN_THROTTLE(30, "Edge aware algorithms currently only support GRBG8 Bayer. "
00202                                 "Falling back to bilinear interpolation.");
00203           algorithm = Debayer_Bilinear;
00204         }
00205         else
00206         {
00207           if (algorithm == Debayer_EdgeAware)
00208             debayerEdgeAware(bayer, color);
00209           else
00210             debayerEdgeAwareWeighted(bayer, color);
00211         }
00212       }
00213       if (algorithm == Debayer_Bilinear ||
00214           algorithm == Debayer_VNG)
00215       {
00216         int code = -1;
00217         if (raw_msg->encoding == enc::BAYER_RGGB8 ||
00218             raw_msg->encoding == enc::BAYER_RGGB16)
00219           code = CV_BayerBG2BGR;
00220         else if (raw_msg->encoding == enc::BAYER_BGGR8 ||
00221                  raw_msg->encoding == enc::BAYER_BGGR16)
00222           code = CV_BayerRG2BGR;
00223         else if (raw_msg->encoding == enc::BAYER_GBRG8 ||
00224                  raw_msg->encoding == enc::BAYER_GBRG16)
00225           code = CV_BayerGR2BGR;
00226         else if (raw_msg->encoding == enc::BAYER_GRBG8 ||
00227                  raw_msg->encoding == enc::BAYER_GRBG16)
00228           code = CV_BayerGB2BGR;
00229 
00230         if (algorithm == Debayer_VNG)
00231           code += CV_BayerBG2BGR_VNG - CV_BayerBG2BGR;
00232 
00233         cv::cvtColor(bayer, color, code);
00234       }
00235       
00236       pub_color_.publish(color_msg);
00237   }
00238   else if (raw_msg->encoding == enc::YUV422)
00239   {
00240     // Use cv_bridge to convert to BGR8
00241     sensor_msgs::ImagePtr color_msg;
00242     try
00243     {
00244       color_msg = cv_bridge::toCvCopy(raw_msg, enc::BGR8)->toImageMsg();
00245       pub_color_.publish(color_msg);
00246     }
00247     catch (cv_bridge::Exception &e)
00248     {
00249       NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
00250     }
00251   }
00252   else if (raw_msg->encoding == enc::TYPE_8UC3)
00253   {
00254     // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...?
00255     NODELET_ERROR_THROTTLE(10,
00256                            "Raw image topic '%s' has ambiguous encoding '8UC3'. The "
00257                            "source should set the encoding to 'bgr8' or 'rgb8'.",
00258                            sub_raw_.getTopic().c_str());
00259   }
00260   else
00261   {
00262     NODELET_ERROR_THROTTLE(10, "Raw image topic '%s' has unsupported encoding '%s'",
00263                            sub_raw_.getTopic().c_str(), raw_msg->encoding.c_str());
00264   }
00265 }
00266 
00267 void DebayerNodelet::configCb(Config &config, uint32_t level)
00268 {
00269   config_ = config;
00270 }
00271 
00272 } // namespace image_proc
00273 
00274 // Register nodelet
00275 #include <pluginlib/class_list_macros.h>
00276 PLUGINLIB_EXPORT_CLASS( image_proc::DebayerNodelet, nodelet::Nodelet)


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Aug 26 2015 11:57:28