Point Cloud Library (PCL)  1.14.1-dev
organized_pointcloud_compression.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef ORGANIZED_COMPRESSION_HPP
39 #define ORGANIZED_COMPRESSION_HPP
40 
41 #include <pcl/compression/organized_pointcloud_compression.h>
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_cloud.h>
45 
46 #include <pcl/compression/libpng_wrapper.h>
47 #include <pcl/compression/organized_pointcloud_conversion.h>
48 
49 #include <vector>
50 #include <cassert>
51 
52 namespace pcl
53 {
54  namespace io
55  {
56  //////////////////////////////////////////////////////////////////////////////////////////////
57  template<typename PointT> void
59  std::ostream& compressedDataOut_arg,
60  bool doColorEncoding,
61  bool convertToMono,
62  bool bShowStatistics_arg,
63  int pngLevel_arg)
64  {
65  std::uint32_t cloud_width = cloud_arg->width;
66  std::uint32_t cloud_height = cloud_arg->height;
67 
68  float maxDepth, focalLength, disparityShift, disparityScale;
69 
70  // no disparity scaling/shifting required during decoding
71  disparityScale = 1.0f;
72  disparityShift = 0.0f;
73 
74  analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength);
75 
76  // encode header identifier
77  compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
78  // encode point cloud width
79  compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_width), sizeof (cloud_width));
80  // encode frame type height
81  compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_height), sizeof (cloud_height));
82  // encode frame max depth
83  compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
84  // encode frame focal length
85  compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength), sizeof (focalLength));
86  // encode frame disparity scale
87  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale), sizeof (disparityScale));
88  // encode frame disparity shift
89  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift), sizeof (disparityShift));
90 
91  // disparity and rgb image data
92  std::vector<std::uint16_t> disparityData;
93  std::vector<std::uint8_t> colorData;
94 
95  // compressed disparity and rgb image data
96  std::vector<std::uint8_t> compressedDisparity;
97  std::vector<std::uint8_t> compressedColor;
98 
99  std::uint32_t compressedDisparitySize = 0;
100  std::uint32_t compressedColorSize = 0;
101 
102  // Convert point cloud to disparity and rgb image
103  OrganizedConversion<PointT>::convert (*cloud_arg, focalLength, disparityShift, disparityScale, convertToMono, disparityData, colorData);
104 
105  // Compress disparity information
106  encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg);
107 
108  compressedDisparitySize = static_cast<std::uint32_t>(compressedDisparity.size());
109  // Encode size of compressed disparity image data
110  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
111  // Output compressed disparity to ostream
112  compressedDataOut_arg.write (reinterpret_cast<const char*> (compressedDisparity.data()), compressedDisparity.size () * sizeof(std::uint8_t));
113 
114  // Compress color information
115  if (CompressionPointTraits<PointT>::hasColor && doColorEncoding)
116  {
117  if (convertToMono)
118  {
119  encodeMonoImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
120  } else
121  {
122  encodeRGBImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
123  }
124  }
125 
126  compressedColorSize = static_cast<std::uint32_t>(compressedColor.size ());
127  // Encode size of compressed Color image data
128  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
129  // Output compressed disparity to ostream
130  compressedDataOut_arg.write (reinterpret_cast<const char*> (compressedColor.data()), compressedColor.size () * sizeof(std::uint8_t));
131 
132  if (bShowStatistics_arg)
133  {
134  std::uint64_t pointCount = cloud_width * cloud_height;
135  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
136 
137  PCL_INFO("*** POINTCLOUD ENCODING ***\n");
138  PCL_INFO("Number of encoded points: %ld\n", pointCount);
139  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
140  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
141  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
142  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
143  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
144  }
145 
146  // flush output stream
147  compressedDataOut_arg.flush();
148  }
149 
150  //////////////////////////////////////////////////////////////////////////////////////////////
151  template<typename PointT> void
153  std::vector<std::uint8_t>& colorImage_arg,
154  std::uint32_t width_arg,
155  std::uint32_t height_arg,
156  std::ostream& compressedDataOut_arg,
157  bool doColorEncoding,
158  bool convertToMono,
159  bool bShowStatistics_arg,
160  int pngLevel_arg,
161  float focalLength_arg,
162  float disparityShift_arg,
163  float disparityScale_arg)
164  {
165  float maxDepth = -1;
166 
167  std::size_t cloud_size = width_arg*height_arg;
168  assert (disparityMap_arg.size()==cloud_size);
169  if (!colorImage_arg.empty ())
170  {
171  assert (colorImage_arg.size()==cloud_size*3);
172  }
173 
174  // encode header identifier
175  compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
176  // encode point cloud width
177  compressedDataOut_arg.write (reinterpret_cast<const char*> (&width_arg), sizeof (width_arg));
178  // encode frame type height
179  compressedDataOut_arg.write (reinterpret_cast<const char*> (&height_arg), sizeof (height_arg));
180  // encode frame max depth
181  compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
182  // encode frame focal length
183  compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength_arg), sizeof (focalLength_arg));
184  // encode frame disparity scale
185  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale_arg), sizeof (disparityScale_arg));
186  // encode frame disparity shift
187  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift_arg), sizeof (disparityShift_arg));
188 
189  // compressed disparity and rgb image data
190  std::vector<std::uint8_t> compressedDisparity;
191  std::vector<std::uint8_t> compressedColor;
192 
193  std::uint32_t compressedDisparitySize = 0;
194  std::uint32_t compressedColorSize = 0;
195 
196  // Remove color information of invalid points
197  std::uint16_t* depth_ptr = disparityMap_arg.data();
198  std::uint8_t* color_ptr = colorImage_arg.data();
199 
200  for (std::size_t i = 0; i < cloud_size; ++i, ++depth_ptr, color_ptr += sizeof(std::uint8_t) * 3)
201  {
202  if (!(*depth_ptr) || (*depth_ptr==0x7FF)) {
203  std::fill_n(color_ptr, 3, 0);
204  }
205  }
206 
207  // Compress disparity information
208  encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
209 
210  compressedDisparitySize = static_cast<std::uint32_t>(compressedDisparity.size());
211  // Encode size of compressed disparity image data
212  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
213  // Output compressed disparity to ostream
214  compressedDataOut_arg.write (reinterpret_cast<const char*> (compressedDisparity.data()), compressedDisparity.size () * sizeof(std::uint8_t));
215 
216  // Compress color information
217  if (!colorImage_arg.empty () && doColorEncoding)
218  {
219  if (convertToMono)
220  {
221  std::vector<std::uint8_t> monoImage;
222  std::size_t size = width_arg*height_arg;
223 
224  monoImage.reserve(size);
225 
226  // grayscale conversion
227  for (std::size_t i = 0; i < size; ++i)
228  {
229  auto grayvalue = static_cast<std::uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) +
230  0.5870 * static_cast<float>(colorImage_arg[i*3+1]) +
231  0.1140 * static_cast<float>(colorImage_arg[i*3+2]));
232  monoImage.push_back(grayvalue);
233  }
234  encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
235 
236  } else
237  {
238  encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
239  }
240 
241  }
242 
243  compressedColorSize = static_cast<std::uint32_t>(compressedColor.size ());
244  // Encode size of compressed Color image data
245  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
246  // Output compressed disparity to ostream
247  compressedDataOut_arg.write (reinterpret_cast<const char*> (compressedColor.data()), compressedColor.size () * sizeof(std::uint8_t));
248 
249  if (bShowStatistics_arg)
250  {
251  std::uint64_t pointCount = width_arg * height_arg;
252  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
253 
254  PCL_INFO("*** POINTCLOUD ENCODING ***\n");
255  PCL_INFO("Number of encoded points: %ld\n", pointCount);
256  PCL_INFO("Size of uncompressed disparity map+color image: %.2f kBytes\n", (static_cast<float> (pointCount) * (sizeof(std::uint8_t)*3+sizeof(std::uint16_t))) / 1024.0f);
257  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
258  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
259  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(std::uint8_t)*3+sizeof(std::uint16_t)) * 100.0f);
260  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
261  }
262 
263  // flush output stream
264  compressedDataOut_arg.flush();
265  }
266 
267  //////////////////////////////////////////////////////////////////////////////////////////////
268  template<typename PointT> bool
269  OrganizedPointCloudCompression<PointT>::decodePointCloud (std::istream& compressedDataIn_arg,
270  PointCloudPtr &cloud_arg,
271  bool bShowStatistics_arg)
272  {
273  std::uint32_t cloud_width;
274  std::uint32_t cloud_height;
275  float maxDepth;
276  float focalLength;
277  float disparityShift = 0.0f;
278  float disparityScale;
279 
280  // disparity and rgb image data
281  std::vector<std::uint16_t> disparityData;
282  std::vector<std::uint8_t> colorData;
283 
284  // compressed disparity and rgb image data
285  std::vector<std::uint8_t> compressedDisparity;
286  std::vector<std::uint8_t> compressedColor;
287 
288  std::uint32_t compressedDisparitySize;
289  std::uint32_t compressedColorSize;
290 
291  // PNG decoded parameters
292  unsigned int png_channels = 1;
293 
294  // sync to frame header
295  unsigned int headerIdPos = 0;
296  bool valid_stream = true;
297  while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
298  {
299  char readChar;
300  compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
301  if (compressedDataIn_arg.gcount()!= sizeof (readChar))
302  valid_stream = false;
303  if (readChar != frameHeaderIdentifier_[headerIdPos++])
304  headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
305 
306  valid_stream &= compressedDataIn_arg.good ();
307  }
308 
309  if (valid_stream) {
310 
311  //////////////
312  // reading frame header
313  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
314  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
315  compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
316  compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
317  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
318  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
319 
320  // reading compressed disparity data
321  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
322  compressedDisparity.resize (compressedDisparitySize);
323  compressedDataIn_arg.read (reinterpret_cast<char*> (compressedDisparity.data()), compressedDisparitySize * sizeof(std::uint8_t));
324 
325  // reading compressed rgb data
326  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
327  compressedColor.resize (compressedColorSize);
328  compressedDataIn_arg.read (reinterpret_cast<char*> (compressedColor.data()), compressedColorSize * sizeof(std::uint8_t));
329 
330  std::size_t png_width = 0;
331  std::size_t png_height = 0;
332 
333  // decode PNG compressed disparity data
334  decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
335 
336  // decode PNG compressed rgb data
337  decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
338  } else {
339  PCL_ERROR("[OrganizedPointCloudCompression::decodePointCloud] Unable to find an encoded point cloud in the input stream!\n");
340  return false;
341  }
342 
343  if (disparityShift==0.0f)
344  {
345  // reconstruct point cloud
347  colorData,
348  (png_channels == 1),
349  cloud_width,
350  cloud_height,
351  focalLength,
352  disparityShift,
353  disparityScale,
354  *cloud_arg);
355  } else
356  {
357 
358  // we need to decode a raw shift image
359  std::size_t size = disparityData.size();
360  std::vector<float> depthData;
361  depthData.resize(size);
362 
363  // initialize shift-to-depth converter
364  if (!sd_converter_.isInitialized())
365  sd_converter_.generateLookupTable();
366 
367  // convert shift to depth image
368  for (std::size_t i=0; i<size; ++i)
369  depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
370 
371  // reconstruct point cloud
373  colorData,
374  static_cast<bool>(png_channels==1),
375  cloud_width,
376  cloud_height,
377  focalLength,
378  *cloud_arg);
379  }
380 
381  if (bShowStatistics_arg)
382  {
383  std::uint64_t pointCount = cloud_width * cloud_height;
384  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
385 
386  PCL_INFO("*** POINTCLOUD DECODING ***\n");
387  PCL_INFO("Number of encoded points: %ld\n", pointCount);
388  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
389  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
390  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
391  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
392  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
393  }
394 
395  return valid_stream;
396  }
397 
398  //////////////////////////////////////////////////////////////////////////////////////////////
399  template<typename PointT> void
401  float& maxDepth_arg,
402  float& focalLength_arg) const
403  {
404  std::size_t width = cloud_arg->width;
405  std::size_t height = cloud_arg->height;
406 
407  // Center of organized point cloud
408  int centerX = static_cast<int> (width / 2);
409  int centerY = static_cast<int> (height / 2);
410 
411  // Ensure we have an organized point cloud
412  assert((width>1) && (height>1));
413  assert(width*height == cloud_arg->size());
414 
415  float maxDepth = 0;
416  float focalLength = 0;
417 
418  std::size_t it = 0;
419  for (int y = -centerY; y < centerY; ++y )
420  for (int x = -centerX; x < centerX; ++x )
421  {
422  const PointT& point = (*cloud_arg)[it++];
423 
424  if (pcl::isFinite (point))
425  {
426  if (maxDepth < point.z)
427  {
428  // Update maximum depth
429  maxDepth = point.z;
430 
431  // Calculate focal length
432  focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
433  }
434  }
435  }
436 
437  // Update return values
438  maxDepth_arg = maxDepth;
439  focalLength_arg = focalLength;
440  }
441 
442  }
443 }
444 
445 #endif
446 
void encodePointCloud(const PointCloudConstPtr &cloud_arg, std::ostream &compressedDataOut_arg, bool doColorEncoding=false, bool convertToMono=false, bool bShowStatistics_arg=true, int pngLevel_arg=-1)
Encode point cloud to output stream.
bool decodePointCloud(std::istream &compressedDataIn_arg, PointCloudPtr &cloud_arg, bool bShowStatistics_arg=true)
Decode point cloud from input stream.
void analyzeOrganizedCloud(PointCloudConstPtr cloud_arg, float &maxDepth_arg, float &focalLength_arg) const
Analyze input point cloud and calculate the maximum depth and focal length.
void encodeRawDisparityMapWithColorImage(std::vector< std::uint16_t > &disparityMap_arg, std::vector< std::uint8_t > &colorImage_arg, std::uint32_t width_arg, std::uint32_t height_arg, std::ostream &compressedDataOut_arg, bool doColorEncoding=false, bool convertToMono=false, bool bShowStatistics_arg=true, int pngLevel_arg=-1, float focalLength_arg=525.0f, float disparityShift_arg=174.825f, float disparityScale_arg=-0.161175f)
Encode raw disparity map and color image.
PCL_EXPORTS void decodePNGToImage(std::vector< std::uint8_t > &pngData_arg, std::vector< std::uint8_t > &imageData_arg, std::size_t &width_arg, std::size_t &heigh_argt, unsigned int &channels_arg)
Decode compressed PNG to 8-bit image.
PCL_EXPORTS void encodeRGBImageToPNG(std::vector< std::uint8_t > &image_arg, std::size_t width_arg, std::size_t height_arg, std::vector< std::uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit RGB image to PNG format.
PCL_EXPORTS void encodeMonoImageToPNG(std::vector< std::uint8_t > &image_arg, std::size_t width_arg, std::size_t height_arg, std::vector< std::uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit mono image to PNG format.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.