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
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
00043 sensor_msgs::CompressedImage compressed;
00044 compressed.header = message.header;
00045 compressed.format = message.encoding;
00046
00047
00048 std::vector<int> params;
00049 params.resize(3, 0);
00050
00051
00052 int bitDepth = enc::bitDepth(message.encoding);
00053 int numChannels = enc::numChannels(message.encoding);
00054
00055
00056 ConfigHeader compressionConfig;
00057 compressionConfig.format = INV_DEPTH;
00058
00059
00060 vector<uint8_t> compressedImage;
00061
00062
00063 compressed.format += "; compressedDepth";
00064
00065
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
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
00092 Mat invDepthImg(rows, cols, CV_16UC1);
00093
00094
00095 float depthQuantA = depthZ0 * (depthZ0 + 1.0f);
00096 float depthQuantB = 1.0f - depthQuantA / depthMax;
00097
00098
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
00105 for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg)
00106 {
00107
00108 if (*itDepthImg < depthMax)
00109 {
00110 *itInvDepthImg = depthQuantA / *itDepthImg + depthQuantB;
00111 }
00112 else
00113 {
00114 *itInvDepthImg = 0;
00115 }
00116 }
00117
00118
00119 compressionConfig.depthParam[0] = depthQuantA;
00120 compressionConfig.depthParam[1] = depthQuantB;
00121
00122 try
00123 {
00124
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
00144 else if ((bitDepth == 16) && (numChannels == 1))
00145 {
00146
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
00166 MatIterator_<unsigned short> itDepthImg = cv_ptr->image.begin<unsigned short>(),
00167 itDepthImg_end = cv_ptr->image.end<unsigned short>();
00168
00169
00170 for (; itDepthImg != itDepthImg_end; ++itDepthImg)
00171 {
00172 if (*itDepthImg > depthMaxUShort)
00173 *itDepthImg = 0;
00174 }
00175
00176
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
00195 compressed.data.resize(sizeof(ConfigHeader));
00196 memcpy(&compressed.data[0], &compressionConfig, sizeof(ConfigHeader));
00197
00198
00199 compressed.data.insert(compressed.data.end(), compressedImage.begin(), compressedImage.end());
00200
00201
00202 publish_fn(compressed);
00203 }
00204
00205 }
00206
00207 }