compressed_depth_subscriber.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_subscriber.h"
00036 #include <sensor_msgs/image_encodings.h>
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <opencv/cvwimage.h>
00039 #include <opencv/highgui.h>
00040 #include <opencv2/imgproc/imgproc.hpp>
00041 
00042 #include "compressed_depth_image_transport/compression_common.h"
00043 
00044 #include <limits>
00045 #include <vector>
00046 
00047 using namespace cv;
00048 
00049 namespace enc = sensor_msgs::image_encodings;
00050 
00051 namespace compressed_depth_image_transport
00052 {
00053 
00054 void CompressedDepthSubscriber::internalCallback(const sensor_msgs::CompressedImageConstPtr& message,
00055                                             const Callback& user_cb)
00056 
00057 {
00058 
00059   cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
00060 
00061   // Copy message header
00062   cv_ptr->header = message->header;
00063 
00064   // Assign image encoding
00065   string image_encoding = message->format.substr(0, message->format.find(';'));
00066   cv_ptr->encoding = image_encoding;
00067 
00068   // Decode message data
00069   if (message->data.size() > sizeof(ConfigHeader))
00070   {
00071 
00072     // Read compression type from stream
00073     ConfigHeader compressionConfig;
00074     memcpy(&compressionConfig, &message->data[0], sizeof(compressionConfig));
00075 
00076     // Get compressed image data
00077     const vector<uint8_t> imageData(message->data.begin() + sizeof(compressionConfig), message->data.end());
00078 
00079     // Depth map decoding
00080     float depthQuantA, depthQuantB;
00081 
00082     // Read quantization parameters
00083     depthQuantA = compressionConfig.depthParam[0];
00084     depthQuantB = compressionConfig.depthParam[1];
00085 
00086     if (enc::bitDepth(image_encoding) == 32)
00087     {
00088       cv::Mat decompressed;
00089       try
00090       {
00091         // Decode image data
00092         decompressed = cv::imdecode(imageData, CV_LOAD_IMAGE_UNCHANGED);
00093       }
00094       catch (cv::Exception& e)
00095       {
00096         ROS_ERROR("%s", e.what());
00097       }
00098 
00099       size_t rows = decompressed.rows;
00100       size_t cols = decompressed.cols;
00101 
00102       if ((rows > 0) && (cols > 0))
00103       {
00104         cv_ptr->image = Mat(rows, cols, CV_32FC1);
00105 
00106         // Depth conversion
00107         MatIterator_<float> itDepthImg = cv_ptr->image.begin<float>(),
00108                             itDepthImg_end = cv_ptr->image.end<float>();
00109         MatConstIterator_<unsigned short> itInvDepthImg = decompressed.begin<unsigned short>(),
00110                                           itInvDepthImg_end = decompressed.end<unsigned short>();
00111 
00112         for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg)
00113         {
00114           // check for NaN & max depth
00115           if (*itInvDepthImg)
00116           {
00117             *itDepthImg = depthQuantA / ((float)*itInvDepthImg - depthQuantB);
00118           }
00119           else
00120           {
00121             *itDepthImg = std::numeric_limits<float>::quiet_NaN();
00122           }
00123         }
00124 
00125         // Publish message to user callback
00126         user_cb(cv_ptr->toImageMsg());
00127       }
00128     }
00129     else
00130     {
00131       // Decode raw image
00132       try
00133       {
00134         cv_ptr->image = cv::imdecode(imageData, CV_LOAD_IMAGE_UNCHANGED);
00135       }
00136       catch (cv::Exception& e)
00137       {
00138         ROS_ERROR("%s", e.what());
00139       }
00140 
00141       size_t rows = cv_ptr->image.rows;
00142       size_t cols = cv_ptr->image.cols;
00143 
00144       if ((rows > 0) && (cols > 0))
00145         // Publish message to user callback
00146         user_cb(cv_ptr->toImageMsg());
00147     }
00148   }
00149 }
00150 
00151 } //namespace compressed_depth_image_transport


compressed_depth_image_transport
Author(s): Julius Kammerl
autogenerated on Mon Oct 6 2014 00:47:16