Point Cloud Library (PCL)  1.7.1
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/common/boost.h>
47 #include <pcl/common/eigen.h>
48 #include <pcl/common/common.h>
49 #include <pcl/common/io.h>
50 
51 #include <pcl/compression/libpng_wrapper.h>
52 #include <pcl/compression/organized_pointcloud_conversion.h>
53 
54 #include <string>
55 #include <vector>
56 #include <limits>
57 #include <assert.h>
58 
59 namespace pcl
60 {
61  namespace io
62  {
63  //////////////////////////////////////////////////////////////////////////////////////////////
64  template<typename PointT> void
66  std::ostream& compressedDataOut_arg,
67  bool doColorEncoding,
68  bool convertToMono,
69  bool bShowStatistics_arg,
70  int pngLevel_arg)
71  {
72  uint32_t cloud_width = cloud_arg->width;
73  uint32_t cloud_height = cloud_arg->height;
74 
75  float maxDepth, focalLength, disparityShift, disparityScale;
76 
77  // no disparity scaling/shifting required during decoding
78  disparityScale = 1.0f;
79  disparityShift = 0.0f;
80 
81  analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength);
82 
83  // encode header identifier
84  compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
85  // encode point cloud width
86  compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_width), sizeof (cloud_width));
87  // encode frame type height
88  compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_height), sizeof (cloud_height));
89  // encode frame max depth
90  compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
91  // encode frame focal lenght
92  compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength), sizeof (focalLength));
93  // encode frame disparity scale
94  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale), sizeof (disparityScale));
95  // encode frame disparity shift
96  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift), sizeof (disparityShift));
97 
98  // disparity and rgb image data
99  std::vector<uint16_t> disparityData;
100  std::vector<uint8_t> colorData;
101 
102  // compressed disparity and rgb image data
103  std::vector<uint8_t> compressedDisparity;
104  std::vector<uint8_t> compressedColor;
105 
106  uint32_t compressedDisparitySize = 0;
107  uint32_t compressedColorSize = 0;
108 
109  // Convert point cloud to disparity and rgb image
110  OrganizedConversion<PointT>::convert (*cloud_arg, focalLength, disparityShift, disparityScale, convertToMono, disparityData, colorData);
111 
112  // Compress disparity information
113  encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg);
114 
115  compressedDisparitySize = static_cast<uint32_t>(compressedDisparity.size());
116  // Encode size of compressed disparity image data
117  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
118  // Output compressed disparity to ostream
119  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(uint8_t));
120 
121  // Compress color information
122  if (CompressionPointTraits<PointT>::hasColor && doColorEncoding)
123  {
124  if (convertToMono)
125  {
126  encodeMonoImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
127  } else
128  {
129  encodeRGBImageToPNG (colorData, cloud_width, cloud_height, compressedColor, 1 /*Z_BEST_SPEED*/);
130  }
131  }
132 
133  compressedColorSize = static_cast<uint32_t>(compressedColor.size ());
134  // Encode size of compressed Color image data
135  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
136  // Output compressed disparity to ostream
137  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(uint8_t));
138 
139  if (bShowStatistics_arg)
140  {
141  uint64_t pointCount = cloud_width * cloud_height;
142  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
143 
144  PCL_INFO("*** POINTCLOUD ENCODING ***\n");
145  PCL_INFO("Number of encoded points: %ld\n", pointCount);
146  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
147  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
148  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
149  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
150  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
151  }
152 
153  // flush output stream
154  compressedDataOut_arg.flush();
155  }
156 
157  //////////////////////////////////////////////////////////////////////////////////////////////
158  template<typename PointT> void
160  std::vector<uint8_t>& colorImage_arg,
161  uint32_t width_arg,
162  uint32_t height_arg,
163  std::ostream& compressedDataOut_arg,
164  bool doColorEncoding,
165  bool convertToMono,
166  bool bShowStatistics_arg,
167  int pngLevel_arg,
168  float focalLength_arg,
169  float disparityShift_arg,
170  float disparityScale_arg)
171  {
172  float maxDepth = -1;
173 
174  size_t cloud_size = width_arg*height_arg;
175  assert (disparityMap_arg.size()==cloud_size);
176  if (colorImage_arg.size())
177  {
178  assert (colorImage_arg.size()==cloud_size*3);
179  }
180 
181  // encode header identifier
182  compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
183  // encode point cloud width
184  compressedDataOut_arg.write (reinterpret_cast<const char*> (&width_arg), sizeof (width_arg));
185  // encode frame type height
186  compressedDataOut_arg.write (reinterpret_cast<const char*> (&height_arg), sizeof (height_arg));
187  // encode frame max depth
188  compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth), sizeof (maxDepth));
189  // encode frame focal lenght
190  compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength_arg), sizeof (focalLength_arg));
191  // encode frame disparity scale
192  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale_arg), sizeof (disparityScale_arg));
193  // encode frame disparity shift
194  compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift_arg), sizeof (disparityShift_arg));
195 
196  // compressed disparity and rgb image data
197  std::vector<uint8_t> compressedDisparity;
198  std::vector<uint8_t> compressedColor;
199 
200  uint32_t compressedDisparitySize = 0;
201  uint32_t compressedColorSize = 0;
202 
203  // Remove color information of invalid points
204  uint16_t* depth_ptr = &disparityMap_arg[0];
205  uint8_t* color_ptr = &colorImage_arg[0];
206 
207  size_t i;
208  for (i=0; i<cloud_size; ++i, ++depth_ptr, color_ptr+=sizeof(uint8_t)*3)
209  {
210  if (!(*depth_ptr) || (*depth_ptr==0x7FF))
211  memset(color_ptr, 0, sizeof(uint8_t)*3);
212  }
213 
214  // Compress disparity information
215  encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
216 
217  compressedDisparitySize = static_cast<uint32_t>(compressedDisparity.size());
218  // Encode size of compressed disparity image data
219  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
220  // Output compressed disparity to ostream
221  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(uint8_t));
222 
223  // Compress color information
224  if (colorImage_arg.size() && doColorEncoding)
225  {
226  if (convertToMono)
227  {
228  size_t i, size;
229  vector<uint8_t> monoImage;
230  size = width_arg*height_arg;
231 
232  monoImage.reserve(size);
233 
234  // grayscale conversion
235  for (i=0; i<size; ++i)
236  {
237  uint8_t grayvalue = static_cast<uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) +
238  0.5870 * static_cast<float>(colorImage_arg[i*3+1]) +
239  0.1140 * static_cast<float>(colorImage_arg[i*3+2]));
240  monoImage.push_back(grayvalue);
241  }
242  encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
243 
244  } else
245  {
246  encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
247  }
248 
249  }
250 
251  compressedColorSize = static_cast<uint32_t>(compressedColor.size ());
252  // Encode size of compressed Color image data
253  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
254  // Output compressed disparity to ostream
255  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(uint8_t));
256 
257  if (bShowStatistics_arg)
258  {
259  uint64_t pointCount = width_arg * height_arg;
260  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
261 
262  PCL_INFO("*** POINTCLOUD ENCODING ***\n");
263  PCL_INFO("Number of encoded points: %ld\n", pointCount);
264  PCL_INFO("Size of uncompressed disparity map+color image: %.2f kBytes\n", (static_cast<float> (pointCount) * (sizeof(uint8_t)*3+sizeof(uint16_t))) / 1024.0f);
265  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
266  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
267  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(uint8_t)*3+sizeof(uint16_t)) * 100.0f);
268  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
269  }
270 
271  // flush output stream
272  compressedDataOut_arg.flush();
273  }
274 
275  //////////////////////////////////////////////////////////////////////////////////////////////
276  template<typename PointT> bool
277  OrganizedPointCloudCompression<PointT>::decodePointCloud (std::istream& compressedDataIn_arg,
278  PointCloudPtr &cloud_arg,
279  bool bShowStatistics_arg)
280  {
281  uint32_t cloud_width;
282  uint32_t cloud_height;
283  float maxDepth, focalLength, disparityShift, disparityScale;
284 
285  // disparity and rgb image data
286  std::vector<uint16_t> disparityData;
287  std::vector<uint8_t> colorData;
288 
289  // compressed disparity and rgb image data
290  std::vector<uint8_t> compressedDisparity;
291  std::vector<uint8_t> compressedColor;
292 
293  uint32_t compressedDisparitySize;
294  uint32_t compressedColorSize;
295 
296  // PNG decoded parameters
297  size_t png_width = 0;
298  size_t png_height = 0;
299  unsigned int png_channels = 1;
300 
301  // sync to frame header
302  unsigned int headerIdPos = 0;
303  bool valid_stream = true;
304  while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
305  {
306  char readChar;
307  compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
308  if (compressedDataIn_arg.gcount()!= sizeof (readChar))
309  valid_stream = false;
310  if (readChar != frameHeaderIdentifier_[headerIdPos++])
311  headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
312 
313  valid_stream &= compressedDataIn_arg.good ();
314  }
315 
316  if (valid_stream) {
317 
318  //////////////
319  // reading frame header
320  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
321  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
322  compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
323  compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
324  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
325  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
326 
327  // reading compressed disparity data
328  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
329  compressedDisparity.resize (compressedDisparitySize);
330  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(uint8_t));
331 
332  // reading compressed rgb data
333  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
334  compressedColor.resize (compressedColorSize);
335  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(uint8_t));
336 
337  // decode PNG compressed disparity data
338  decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
339 
340  // decode PNG compressed rgb data
341  decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
342  }
343 
344  if (disparityShift==0.0f)
345  {
346  // reconstruct point cloud
348  colorData,
349  static_cast<bool>(png_channels==1),
350  cloud_width,
351  cloud_height,
352  focalLength,
353  disparityShift,
354  disparityScale,
355  *cloud_arg);
356  } else
357  {
358 
359  // we need to decode a raw shift image
360  std::size_t size = disparityData.size();
361  std::vector<float> depthData;
362  depthData.resize(size);
363 
364  // initialize shift-to-depth converter
365  if (!sd_converter_.isInitialized())
366  sd_converter_.generateLookupTable();
367 
368  // convert shift to depth image
369  for (std::size_t i=0; i<size; ++i)
370  depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
371 
372  // reconstruct point cloud
374  colorData,
375  static_cast<bool>(png_channels==1),
376  cloud_width,
377  cloud_height,
378  focalLength,
379  *cloud_arg);
380  }
381 
382  if (bShowStatistics_arg)
383  {
384  uint64_t pointCount = cloud_width * cloud_height;
385  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
386 
387  PCL_INFO("*** POINTCLOUD DECODING ***\n");
388  PCL_INFO("Number of encoded points: %ld\n", pointCount);
389  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
390  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
391  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
392  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
393  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
394  }
395 
396  return valid_stream;
397  }
398 
399  //////////////////////////////////////////////////////////////////////////////////////////////
400  template<typename PointT> void
402  float& maxDepth_arg,
403  float& focalLength_arg) const
404  {
405  size_t width, height, it;
406  int centerX, centerY;
407  int x, y;
408  float maxDepth;
409  float focalLength;
410 
411  width = cloud_arg->width;
412  height = cloud_arg->height;
413 
414  // Center of organized point cloud
415  centerX = static_cast<int> (width / 2);
416  centerY = static_cast<int> (height / 2);
417 
418  // Ensure we have an organized point cloud
419  assert((width>1) && (height>1));
420  assert(width*height == cloud_arg->points.size());
421 
422  maxDepth = 0;
423  focalLength = 0;
424 
425  it = 0;
426  for (y = -centerY; y < +centerY; ++y)
427  for (x = -centerX; x < +centerX; ++x)
428  {
429  const PointT& point = cloud_arg->points[it++];
430 
431  if (pcl::isFinite (point))
432  {
433  if (maxDepth < point.z)
434  {
435  // Update maximum depth
436  maxDepth = point.z;
437 
438  // Calculate focal length
439  focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
440  }
441  }
442  }
443 
444  // Update return values
445  maxDepth_arg = maxDepth;
446  focalLength_arg = focalLength;
447  }
448 
449  }
450 }
451 
452 #endif
453