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


compressed_depth_image_transport
Author(s): Julius Kammerl
autogenerated on Sat Dec 28 2013 17:06:20