crop_decimate.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/version.hpp>
00035 #if ((BOOST_VERSION / 100) % 1000) >= 53
00036 #include <boost/thread/lock_guard.hpp>
00037 #endif
00038 
00039 #include <ros/ros.h>
00040 #include <nodelet/nodelet.h>
00041 #include <image_transport/image_transport.h>
00042 #include <sensor_msgs/image_encodings.h>
00043 #include <dynamic_reconfigure/server.h>
00044 #include <cv_bridge/cv_bridge.h>
00045 #include <image_proc/CropDecimateConfig.h>
00046 #include <opencv2/imgproc/imgproc.hpp>
00047 
00048 namespace image_proc {
00049 
00050 using namespace cv_bridge; // CvImage, toCvShare
00051 
00052 class CropDecimateNodelet : public nodelet::Nodelet
00053 {
00054   // ROS communication
00055   boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
00056   image_transport::CameraSubscriber sub_;
00057   int queue_size_;
00058 
00059   boost::mutex connect_mutex_;
00060   image_transport::CameraPublisher pub_;
00061 
00062   // Dynamic reconfigure
00063   boost::recursive_mutex config_mutex_;
00064   typedef image_proc::CropDecimateConfig Config;
00065   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00066   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00067   Config config_;
00068 
00069   virtual void onInit();
00070 
00071   void connectCb();
00072 
00073   void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00074                const sensor_msgs::CameraInfoConstPtr& info_msg);
00075 
00076   void configCb(Config &config, uint32_t level);
00077 };
00078 
00079 void CropDecimateNodelet::onInit()
00080 {
00081   ros::NodeHandle& nh         = getNodeHandle();
00082   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00083   ros::NodeHandle nh_in (nh, "camera");
00084   ros::NodeHandle nh_out(nh, "camera_out");
00085   it_in_ .reset(new image_transport::ImageTransport(nh_in));
00086   it_out_.reset(new image_transport::ImageTransport(nh_out));
00087 
00088   // Read parameters
00089   private_nh.param("queue_size", queue_size_, 5);
00090 
00091   // Set up dynamic reconfigure
00092   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
00093   ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2);
00094   reconfigure_server_->setCallback(f);
00095 
00096   // Monitor whether anyone is subscribed to the output
00097   image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this);
00098   ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this);
00099   // Make sure we don't enter connectCb() between advertising and assigning to pub_
00100   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00101   pub_ = it_out_->advertiseCamera("image_raw",  1, connect_cb, connect_cb, connect_cb_info, connect_cb_info);
00102 }
00103 
00104 // Handles (un)subscribing when clients (un)subscribe
00105 void CropDecimateNodelet::connectCb()
00106 {
00107   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00108   if (pub_.getNumSubscribers() == 0)
00109     sub_.shutdown();
00110   else if (!sub_)
00111   {
00112     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00113     sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this, hints);
00114   }
00115 }
00116 
00117 template <typename T>
00118 void debayer2x2toBGR(const cv::Mat& src, cv::Mat& dst, int R, int G1, int G2, int B)
00119 {
00120   typedef cv::Vec<T, 3> DstPixel; // 8- or 16-bit BGR
00121   dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
00122 
00123   int src_row_step = src.step1();
00124   int dst_row_step = dst.step1();
00125   const T* src_row = src.ptr<T>();
00126   T* dst_row = dst.ptr<T>();
00127 
00128   // 2x2 downsample and debayer at once
00129   for (int y = 0; y < dst.rows; ++y)
00130   {
00131     for (int x = 0; x < dst.cols; ++x)
00132     {
00133       dst_row[x*3 + 0] = src_row[x*2 + B];
00134       dst_row[x*3 + 1] = (src_row[x*2 + G1] + src_row[x*2 + G2]) / 2;
00135       dst_row[x*3 + 2] = src_row[x*2 + R];
00136     }
00137     src_row += src_row_step * 2;
00138     dst_row += dst_row_step;
00139   }
00140 }
00141 
00142 // Templated on pixel size, in bytes (MONO8 = 1, BGR8 = 3, RGBA16 = 8, ...)
00143 template <int N>
00144 void decimate(const cv::Mat& src, cv::Mat& dst, int decimation_x, int decimation_y)
00145 {
00146   dst.create(src.rows / decimation_y, src.cols / decimation_x, src.type());
00147 
00148   int src_row_step = src.step[0] * decimation_y;
00149   int src_pixel_step = N * decimation_x;
00150   int dst_row_step = dst.step[0];
00151 
00152   const uint8_t* src_row = src.ptr();
00153   uint8_t* dst_row = dst.ptr();
00154   
00155   for (int y = 0; y < dst.rows; ++y)
00156   {
00157     const uint8_t* src_pixel = src_row;
00158     uint8_t* dst_pixel = dst_row;
00159     for (int x = 0; x < dst.cols; ++x)
00160     {
00161       memcpy(dst_pixel, src_pixel, N); // Should inline with small, fixed N
00162       src_pixel += src_pixel_step;
00163       dst_pixel += N;
00164     }
00165     src_row += src_row_step;
00166     dst_row += dst_row_step;
00167   }
00168 }
00169 
00170 void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
00171                                   const sensor_msgs::CameraInfoConstPtr& info_msg)
00172 {
00175 
00176   Config config;
00177   {
00178     boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00179     config = config_;
00180   }
00181   int decimation_x = config.decimation_x;
00182   int decimation_y = config.decimation_y;
00183 
00184   // Compute the ROI we'll actually use
00185   bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding);
00186   if (is_bayer)
00187   {
00188     // Odd offsets for Bayer images basically change the Bayer pattern, but that's
00189     // unnecessarily complicated to support
00190     config.x_offset &= ~0x1;
00191     config.y_offset &= ~0x1;
00192     config.width &= ~0x1;
00193     config.height &= ~0x1;    
00194   }
00195 
00196   int max_width = image_msg->width - config.x_offset;
00197   int max_height = image_msg->height - config.y_offset;
00198   int width = config.width;
00199   int height = config.height;
00200   if (width == 0 || width > max_width)
00201     width = max_width;
00202   if (height == 0 || height > max_height)
00203     height = max_height;
00204 
00205   // On no-op, just pass the messages along
00206   if (decimation_x == 1               &&
00207       decimation_y == 1               &&
00208       config.x_offset == 0            &&
00209       config.y_offset == 0            &&
00210       width  == (int)image_msg->width &&
00211       height == (int)image_msg->height)
00212   {
00213     pub_.publish(image_msg, info_msg);
00214     return;
00215   }
00216 
00217   // Get a cv::Mat view of the source data
00218   CvImageConstPtr source = toCvShare(image_msg);
00219 
00220   // Except in Bayer downsampling case, output has same encoding as the input
00221   CvImage output(source->header, source->encoding);
00222   // Apply ROI (no copy, still a view of the image_msg data)
00223   output.image = source->image(cv::Rect(config.x_offset, config.y_offset, width, height));
00224 
00225   // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR
00226   if (is_bayer && (decimation_x > 1 || decimation_y > 1))
00227   {
00228     if (decimation_x % 2 != 0 || decimation_y % 2 != 0)
00229     {
00230       NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images");
00231       return;
00232     }
00233 
00234     cv::Mat bgr;
00235     int step = output.image.step1();
00236     if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
00237       debayer2x2toBGR<uint8_t>(output.image, bgr, 0, 1, step, step + 1);
00238     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
00239       debayer2x2toBGR<uint8_t>(output.image, bgr, step + 1, 1, step, 0);
00240     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
00241       debayer2x2toBGR<uint8_t>(output.image, bgr, step, 0, step + 1, 1);
00242     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
00243       debayer2x2toBGR<uint8_t>(output.image, bgr, 1, 0, step + 1, step);
00244     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16)
00245       debayer2x2toBGR<uint16_t>(output.image, bgr, 0, 1, step, step + 1);
00246     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16)
00247       debayer2x2toBGR<uint16_t>(output.image, bgr, step + 1, 1, step, 0);
00248     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16)
00249       debayer2x2toBGR<uint16_t>(output.image, bgr, step, 0, step + 1, 1);
00250     else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16)
00251       debayer2x2toBGR<uint16_t>(output.image, bgr, 1, 0, step + 1, step);
00252     else
00253     {
00254       NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
00255       return;
00256     }
00257 
00258     output.image = bgr;
00259     output.encoding = (bgr.depth() == CV_8U) ? sensor_msgs::image_encodings::BGR8
00260                                              : sensor_msgs::image_encodings::BGR16;
00261     decimation_x /= 2;
00262     decimation_y /= 2;
00263   }
00264 
00265   // Apply further downsampling, if necessary
00266   if (decimation_x > 1 || decimation_y > 1)
00267   {
00268     cv::Mat decimated;
00269 
00270     if (config.interpolation == image_proc::CropDecimate_NN)
00271     {
00272       // Use optimized method instead of OpenCV's more general NN resize
00273       int pixel_size = output.image.elemSize();
00274       switch (pixel_size)
00275       {
00276         // Currently support up through 4-channel float
00277         case 1:
00278           decimate<1>(output.image, decimated, decimation_x, decimation_y);
00279           break;
00280         case 2:
00281           decimate<2>(output.image, decimated, decimation_x, decimation_y);
00282           break;
00283         case 3:
00284           decimate<3>(output.image, decimated, decimation_x, decimation_y);
00285           break;
00286         case 4:
00287           decimate<4>(output.image, decimated, decimation_x, decimation_y);
00288           break;
00289         case 6:
00290           decimate<6>(output.image, decimated, decimation_x, decimation_y);
00291           break;
00292         case 8:
00293           decimate<8>(output.image, decimated, decimation_x, decimation_y);
00294           break;
00295         case 12:
00296           decimate<12>(output.image, decimated, decimation_x, decimation_y);
00297           break;
00298         case 16:
00299           decimate<16>(output.image, decimated, decimation_x, decimation_y);
00300           break;
00301         default:
00302           NODELET_ERROR_THROTTLE(2, "Unsupported pixel size, %d bytes", pixel_size);
00303           return;
00304       }
00305     }
00306     else
00307     {
00308       // Linear, cubic, area, ...
00309       cv::Size size(output.image.cols / decimation_x, output.image.rows / decimation_y);
00310       cv::resize(output.image, decimated, size, 0.0, 0.0, config.interpolation);
00311     }
00312 
00313     output.image = decimated;
00314   }
00315 
00316   // Create output Image message
00318   sensor_msgs::ImagePtr out_image = output.toImageMsg();
00319 
00320   // Create updated CameraInfo message
00321   sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
00322   int binning_x = std::max((int)info_msg->binning_x, 1);
00323   int binning_y = std::max((int)info_msg->binning_y, 1);
00324   out_info->binning_x = binning_x * config.decimation_x;
00325   out_info->binning_y = binning_y * config.decimation_y;
00326   out_info->roi.x_offset += config.x_offset * binning_x;
00327   out_info->roi.y_offset += config.y_offset * binning_y;
00328   out_info->roi.height = height * binning_y;
00329   out_info->roi.width = width * binning_x;
00330   // If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true.
00331   if (width != (int)image_msg->width || height != (int)image_msg->height)
00332     out_info->roi.do_rectify = true;
00333   
00334   pub_.publish(out_image, out_info);
00335 }
00336 
00337 void CropDecimateNodelet::configCb(Config &config, uint32_t level)
00338 {
00339   config_ = config;
00340 }
00341 
00342 } // namespace image_proc
00343 
00344 // Register nodelet
00345 #include <pluginlib/class_list_macros.h>
00346 PLUGINLIB_EXPORT_CLASS( image_proc::CropDecimateNodelet, nodelet::Nodelet)


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