compressed_depth_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 20012, 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 
00035 #include "compressed_depth_image_transport/compressed_depth_publisher.h"
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <sensor_msgs/image_encodings.h>
00038 #include <cv.h>
00039 #include <highgui.h>
00040 #include <boost/make_shared.hpp>
00041 
00042 #include "compressed_depth_image_transport/compression_common.h"
00043 
00044 #include <vector>
00045 #include <sstream>
00046 
00047 using namespace cv;
00048 using namespace std;
00049 
00050 namespace enc = sensor_msgs::image_encodings;
00051 
00052 namespace compressed_depth_image_transport
00053 {
00054 
00055 void CompressedDepthPublisher::advertiseImpl(ros::NodeHandle &nh, const std::string &base_topic, uint32_t queue_size,
00056                                         const image_transport::SubscriberStatusCallback &user_connect_cb,
00057                                         const image_transport::SubscriberStatusCallback &user_disconnect_cb,
00058                                         const ros::VoidPtr &tracked_object, bool latch)
00059 {
00060   typedef image_transport::SimplePublisherPlugin<sensor_msgs::CompressedImage> Base;
00061   Base::advertiseImpl(nh, base_topic, queue_size, user_connect_cb, user_disconnect_cb, tracked_object, latch);
00062 
00063   // Set up reconfigure server for this topic
00064   reconfigure_server_ = boost::make_shared<ReconfigureServer>(this->nh());
00065   ReconfigureServer::CallbackType f = boost::bind(&CompressedDepthPublisher::configCb, this, _1, _2);
00066   reconfigure_server_->setCallback(f);
00067 }
00068 
00069 void CompressedDepthPublisher::configCb(Config& config, uint32_t level)
00070 {
00071   config_ = config;
00072 }
00073 
00074 void CompressedDepthPublisher::publish(const sensor_msgs::Image& message, const PublishFn& publish_fn) const
00075 {
00076   // Compressed image message
00077   sensor_msgs::CompressedImage compressed;
00078   compressed.header = message.header;
00079   compressed.format = message.encoding;
00080 
00081   // Compression settings
00082   std::vector<int> params;
00083   params.resize(3, 0);
00084 
00085   // Bit depth of image encoding
00086   int bitDepth = enc::bitDepth(message.encoding);
00087   int numChannels = enc::numChannels(message.encoding);
00088 
00089   // Image compression configuration
00090   ConfigHeader compressionConfig;
00091   compressionConfig.format = INV_DEPTH;
00092 
00093   // Compressed image data
00094   vector<uint8_t> compressedImage;
00095 
00096   // Update ros message format header
00097   compressed.format += "; compressedDepth";
00098 
00099   // Check input format
00100   if ((bitDepth == 32) && (numChannels == 1))
00101   {
00102     params[0] = CV_IMWRITE_PNG_COMPRESSION;
00103     params[1] = config_.png_level;
00104 
00105     float depthZ0 = config_.depth_quantization;
00106     float depthMax = config_.depth_max;
00107 
00108     // OpenCV-ROS bridge
00109     cv_bridge::CvImagePtr cv_ptr;
00110     try
00111     {
00112       cv_ptr = cv_bridge::toCvCopy(message);
00113     }
00114     catch (cv_bridge::Exception& e)
00115     {
00116       ROS_ERROR("%s", e.what());
00117     }
00118 
00119     const Mat& depthImg = cv_ptr->image;
00120     size_t rows = depthImg.rows;
00121     size_t cols = depthImg.cols;
00122 
00123     if ((rows > 0) && (cols > 0))
00124     {
00125       // Allocate matrix for inverse depth (disparity) coding
00126       Mat invDepthImg(rows, cols, CV_16UC1);
00127 
00128       // Inverse depth quantization parameters
00129       float depthQuantA = depthZ0 * (depthZ0 + 1.0f);
00130       float depthQuantB = 1.0f - depthQuantA / depthMax;
00131 
00132       // Matrix iterators
00133       MatConstIterator_<float> itDepthImg = depthImg.begin<float>(),
00134                                itDepthImg_end = depthImg.end<float>();
00135       MatIterator_<unsigned short> itInvDepthImg = invDepthImg.begin<unsigned short>(),
00136                                    itInvDepthImg_end = invDepthImg.end<unsigned short>();
00137 
00138       // Quantization
00139       for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg)
00140       {
00141         // check for NaN & max depth
00142         if (*itDepthImg < depthMax)
00143         {
00144           *itInvDepthImg = depthQuantA / *itDepthImg + depthQuantB;
00145         }
00146         else
00147         {
00148           *itInvDepthImg = 0;
00149         }
00150       }
00151 
00152       // Add coding parameters to header
00153       compressionConfig.depthParam[0] = depthQuantA;
00154       compressionConfig.depthParam[1] = depthQuantB;
00155 
00156       try
00157       {
00158         // Compress quantized disparity image
00159         if (cv::imencode(".png", invDepthImg, compressedImage, params))
00160         {
00161           float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
00162               / (float)compressedImage.size();
00163           ROS_DEBUG("Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size());
00164         }
00165         else
00166         {
00167           ROS_ERROR("cv::imencode (png) failed on input image");
00168           return;
00169         }
00170       }
00171       catch (cv::Exception& e)
00172       {
00173         ROS_ERROR("%s", e.msg.c_str());
00174       }
00175     }
00176   }
00177   // Raw depth map compression
00178   else if ((bitDepth == 16) && (numChannels == 1))
00179   {
00180     // OpenCV-ROS bridge
00181     cv_bridge::CvImagePtr cv_ptr;
00182     try
00183     {
00184       cv_ptr = cv_bridge::toCvCopy(message);
00185     }
00186     catch (Exception& e)
00187     {
00188       ROS_ERROR("%s", e.msg.c_str());
00189     }
00190 
00191     const Mat& depthImg = cv_ptr->image;
00192     size_t rows = depthImg.rows;
00193     size_t cols = depthImg.cols;
00194 
00195     if ((rows > 0) && (cols > 0))
00196     {
00197       unsigned short depthMaxUShort = static_cast<unsigned short>(config_.depth_max * 1000.0f);
00198 
00199       // Matrix iterators
00200       MatIterator_<unsigned short> itDepthImg = cv_ptr->image.begin<unsigned short>(),
00201                                     itDepthImg_end = cv_ptr->image.end<unsigned short>();
00202 
00203       // Max depth filter
00204       for (; itDepthImg != itDepthImg_end; ++itDepthImg)
00205       {
00206         if (*itDepthImg > depthMaxUShort)
00207           *itDepthImg = 0;
00208       }
00209 
00210       // Compress raw depth image
00211       if (cv::imencode(".png", cv_ptr->image, compressedImage, params))
00212       {
00213         float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
00214             / (float)compressedImage.size();
00215         ROS_DEBUG("Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size());
00216       }
00217       else
00218       {
00219         ROS_ERROR("cv::imencode (png) failed on input image");
00220       }
00221     }
00222   }
00223   else
00224     ROS_ERROR("Compressed Depth Image Transport - Compression requires single-channel 32bit-floating point or 16bit raw depth images (input format is: %s).", message.encoding.c_str());
00225 
00226   if (compressedImage.size() > 0)
00227   {
00228     // Add configuration to binary output
00229     compressed.data.resize(sizeof(ConfigHeader));
00230     memcpy(&compressed.data[0], &compressionConfig, sizeof(ConfigHeader));
00231 
00232     // Add compressed binary data to messages
00233     compressed.data.insert(compressed.data.end(), compressedImage.begin(), compressedImage.end());
00234 
00235     // Publish message
00236     publish_fn(compressed);
00237   }
00238 
00239 }
00240 
00241 } //namespace compressed_depth_image_transport


compressed_depth_image_transport
Author(s): Julius Kammerl
autogenerated on Fri Aug 28 2015 11:08:28