40 #include <opencv2/highgui/highgui.hpp>
49 #ifndef CV_VERSION_EPOCH
50 #include <opencv2/imgcodecs.hpp>
53 #if CV_VERSION_MAJOR > 3
54 #include <opencv2/imgcodecs/legacy/constants_c.h>
70 cv_ptr->header = message.header;
73 const size_t split_pos = message.format.find(
';');
74 const std::string image_encoding = message.format.substr(0, split_pos);
75 std::string compression_format;
77 if (split_pos == std::string::npos) {
78 compression_format =
"png";
80 std::string format = message.format.substr(split_pos);
81 if (format.find(
"compressedDepth png") != std::string::npos) {
82 compression_format =
"png";
83 }
else if (format.find(
"compressedDepth rvl") != std::string::npos) {
84 compression_format =
"rvl";
85 }
else if (format.find(
"compressedDepth") != std::string::npos && format.find(
"compressedDepth ") == std::string::npos) {
86 compression_format =
"png";
88 ROS_ERROR(
"Unsupported image format: %s", message.format.c_str());
89 return sensor_msgs::Image::Ptr();
93 cv_ptr->encoding = image_encoding;
101 memcpy(&compressionConfig, &message.data[0],
sizeof(compressionConfig));
104 const std::vector<uint8_t> imageData(message.data.begin() +
sizeof(compressionConfig), message.data.end());
107 float depthQuantA, depthQuantB;
110 depthQuantA = compressionConfig.
depthParam[0];
111 depthQuantB = compressionConfig.
depthParam[1];
113 if (enc::bitDepth(image_encoding) == 32)
115 cv::Mat decompressed;
116 if (compression_format ==
"png") {
120 decompressed = cv::imdecode(imageData, cv::IMREAD_UNCHANGED);
122 catch (cv::Exception& e)
125 return sensor_msgs::Image::Ptr();
127 }
else if (compression_format ==
"rvl") {
128 const unsigned char *buffer = imageData.data();
131 memcpy(&cols, &buffer[0], 4);
132 memcpy(&rows, &buffer[4], 4);
133 if (rows == 0 || cols == 0)
135 ROS_ERROR_THROTTLE(1.0,
"Received malformed RVL-encoded image. Size %ix%i contains zero.", cols, rows);
136 return sensor_msgs::Image::Ptr();
143 const auto numPixels =
static_cast<uint64_t
>(rows) * cols;
144 if (numPixels > std::numeric_limits<int>::max() || numPixels >
static_cast<uint64_t
>(imageData.size()) * 5)
146 ROS_ERROR_THROTTLE(1.0,
"Received malformed RVL-encoded image. It reports size %ux%u.", cols, rows);
147 return sensor_msgs::Image::Ptr();
150 decompressed = Mat(rows, cols, CV_16UC1);
152 rvl.
DecompressRVL(&buffer[8], decompressed.ptr<
unsigned short>(), cols * rows);
154 return sensor_msgs::Image::Ptr();
157 size_t rows = decompressed.rows;
158 size_t cols = decompressed.cols;
160 if ((rows > 0) && (cols > 0))
162 cv_ptr->image = Mat(rows, cols, CV_32FC1);
165 MatIterator_<float> itDepthImg = cv_ptr->image.begin<
float>(),
166 itDepthImg_end = cv_ptr->image.end<
float>();
167 MatConstIterator_<unsigned short> itInvDepthImg = decompressed.begin<
unsigned short>(),
168 itInvDepthImg_end = decompressed.end<
unsigned short>();
170 for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg)
175 *itDepthImg = depthQuantA / ((float)*itInvDepthImg - depthQuantB);
179 *itDepthImg = std::numeric_limits<float>::quiet_NaN();
184 return cv_ptr->toImageMsg();
190 if (compression_format ==
"png") {
193 cv_ptr->image = cv::imdecode(imageData, CV_LOAD_IMAGE_UNCHANGED);
195 catch (cv::Exception& e)
198 return sensor_msgs::Image::Ptr();
200 }
else if (compression_format ==
"rvl") {
201 const unsigned char *buffer = imageData.data();
203 memcpy(&cols, &buffer[0], 4);
204 memcpy(&rows, &buffer[4], 4);
205 cv_ptr->image = Mat(rows, cols, CV_16UC1);
207 rvl.
DecompressRVL(&buffer[8], cv_ptr->image.ptr<
unsigned short>(), cols * rows);
209 return sensor_msgs::Image::Ptr();
212 size_t rows = cv_ptr->image.rows;
213 size_t cols = cv_ptr->image.cols;
215 if ((rows > 0) && (cols > 0))
218 return cv_ptr->toImageMsg();
222 return sensor_msgs::Image::Ptr();
226 const sensor_msgs::Image& message,
227 const std::string& compression_format,
228 double depth_max,
double depth_quantization,
int png_level)
232 sensor_msgs::CompressedImage::Ptr compressed(
new sensor_msgs::CompressedImage());
233 compressed->header = message.header;
234 compressed->format = message.encoding;
237 std::vector<int> params;
240 int bitDepth = enc::bitDepth(message.encoding);
241 int numChannels = enc::numChannels(message.encoding);
248 std::vector<uint8_t> compressedImage;
251 compressed->format +=
"; compressedDepth " + compression_format;
255 params.emplace_back(cv::IMWRITE_PNG_COMPRESSION);
256 params.emplace_back(png_level);
258 if ((bitDepth == 32) && (numChannels == 1))
260 float depthZ0 = depth_quantization;
261 float depthMax = depth_max;
272 return sensor_msgs::CompressedImage::Ptr();
275 const Mat& depthImg = cv_ptr->image;
276 size_t rows = depthImg.rows;
277 size_t cols = depthImg.cols;
279 if ((rows > 0) && (cols > 0))
282 Mat invDepthImg(rows, cols, CV_16UC1);
285 float depthQuantA = depthZ0 * (depthZ0 + 1.0f);
286 float depthQuantB = 1.0f - depthQuantA / depthMax;
289 MatConstIterator_<float> itDepthImg = depthImg.begin<
float>(),
290 itDepthImg_end = depthImg.end<
float>();
291 MatIterator_<unsigned short> itInvDepthImg = invDepthImg.begin<
unsigned short>(),
292 itInvDepthImg_end = invDepthImg.end<
unsigned short>();
295 for (; (itDepthImg != itDepthImg_end) && (itInvDepthImg != itInvDepthImg_end); ++itDepthImg, ++itInvDepthImg)
298 if (*itDepthImg < depthMax)
300 *itInvDepthImg = depthQuantA / *itDepthImg + depthQuantB;
309 compressionConfig.depthParam[0] = depthQuantA;
310 compressionConfig.depthParam[1] = depthQuantB;
313 if (compression_format ==
"png") {
316 if (cv::imencode(
".png", invDepthImg, compressedImage, params))
318 float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
319 / (
float)compressedImage.size();
320 ROS_DEBUG(
"Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size());
324 ROS_ERROR(
"cv::imencode (png) failed on input image");
325 return sensor_msgs::CompressedImage::Ptr();
328 catch (cv::Exception& e)
331 return sensor_msgs::CompressedImage::Ptr();
333 }
else if (compression_format ==
"rvl") {
334 int numPixels = invDepthImg.rows * invDepthImg.cols;
336 compressedImage.resize(3 * numPixels + 12);
337 uint32_t cols = invDepthImg.cols;
338 uint32_t rows = invDepthImg.rows;
339 memcpy(&compressedImage[0], &cols, 4);
340 memcpy(&compressedImage[4], &rows, 4);
342 int compressedSize = rvl.
CompressRVL(invDepthImg.ptr<
unsigned short>(), &compressedImage[8], numPixels);
343 compressedImage.resize(8 + compressedSize);
348 else if ((bitDepth == 16) && (numChannels == 1))
359 return sensor_msgs::CompressedImage::Ptr();
362 const Mat& depthImg = cv_ptr->image;
363 size_t rows = depthImg.rows;
364 size_t cols = depthImg.cols;
366 if ((rows > 0) && (cols > 0))
368 unsigned short depthMaxUShort =
static_cast<unsigned short>(depth_max * 1000.0f);
371 MatIterator_<unsigned short> itDepthImg = cv_ptr->image.begin<
unsigned short>(),
372 itDepthImg_end = cv_ptr->image.end<
unsigned short>();
375 for (; itDepthImg != itDepthImg_end; ++itDepthImg)
377 if (*itDepthImg > depthMaxUShort)
382 if (compression_format ==
"png") {
383 if (cv::imencode(
".png", cv_ptr->image, compressedImage, params))
385 float cRatio = (float)(cv_ptr->image.rows * cv_ptr->image.cols * cv_ptr->image.elemSize())
386 / (
float)compressedImage.size();
387 ROS_DEBUG(
"Compressed Depth Image Transport - Compression: 1:%.2f (%lu bytes)", cRatio, compressedImage.size());
391 ROS_ERROR(
"cv::imencode (png) failed on input image");
392 return sensor_msgs::CompressedImage::Ptr();
394 }
else if (compression_format ==
"rvl") {
395 int numPixels = cv_ptr->image.rows * cv_ptr->image.cols;
397 compressedImage.resize(3 * numPixels + 12);
398 uint32_t cols = cv_ptr->image.cols;
399 uint32_t rows = cv_ptr->image.rows;
400 memcpy(&compressedImage[0], &cols, 4);
401 memcpy(&compressedImage[4], &rows, 4);
403 int compressedSize = rvl.
CompressRVL(cv_ptr->image.ptr<
unsigned short>(), &compressedImage[8], numPixels);
404 compressedImage.resize(8 + compressedSize);
410 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());
411 return sensor_msgs::CompressedImage::Ptr();
414 if (compressedImage.size() > 0)
418 memcpy(&compressed->data[0], &compressionConfig,
sizeof(
ConfigHeader));
421 compressed->data.insert(compressed->data.end(), compressedImage.begin(), compressedImage.end());
426 return sensor_msgs::CompressedImage::Ptr();