Point Cloud Library (PCL)  1.9.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/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 <cassert>
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 length
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.empty ())
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 length
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  for (size_t i = 0; i < cloud_size; ++i, ++depth_ptr, color_ptr += sizeof(uint8_t) * 3)
208  {
209  if (!(*depth_ptr) || (*depth_ptr==0x7FF))
210  memset(color_ptr, 0, sizeof(uint8_t)*3);
211  }
212 
213  // Compress disparity information
214  encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
215 
216  compressedDisparitySize = static_cast<uint32_t>(compressedDisparity.size());
217  // Encode size of compressed disparity image data
218  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
219  // Output compressed disparity to ostream
220  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(uint8_t));
221 
222  // Compress color information
223  if (!colorImage_arg.empty () && doColorEncoding)
224  {
225  if (convertToMono)
226  {
227  vector<uint8_t> monoImage;
228  size_t size = width_arg*height_arg;
229 
230  monoImage.reserve(size);
231 
232  // grayscale conversion
233  for (size_t i = 0; i < size; ++i)
234  {
235  uint8_t grayvalue = static_cast<uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) +
236  0.5870 * static_cast<float>(colorImage_arg[i*3+1]) +
237  0.1140 * static_cast<float>(colorImage_arg[i*3+2]));
238  monoImage.push_back(grayvalue);
239  }
240  encodeMonoImageToPNG (monoImage, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
241 
242  } else
243  {
244  encodeRGBImageToPNG (colorImage_arg, width_arg, height_arg, compressedColor, 1 /*Z_BEST_SPEED*/);
245  }
246 
247  }
248 
249  compressedColorSize = static_cast<uint32_t>(compressedColor.size ());
250  // Encode size of compressed Color image data
251  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
252  // Output compressed disparity to ostream
253  compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(uint8_t));
254 
255  if (bShowStatistics_arg)
256  {
257  uint64_t pointCount = width_arg * height_arg;
258  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
259 
260  PCL_INFO("*** POINTCLOUD ENCODING ***\n");
261  PCL_INFO("Number of encoded points: %ld\n", pointCount);
262  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);
263  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
264  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
265  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(uint8_t)*3+sizeof(uint16_t)) * 100.0f);
266  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
267  }
268 
269  // flush output stream
270  compressedDataOut_arg.flush();
271  }
272 
273  //////////////////////////////////////////////////////////////////////////////////////////////
274  template<typename PointT> bool
275  OrganizedPointCloudCompression<PointT>::decodePointCloud (std::istream& compressedDataIn_arg,
276  PointCloudPtr &cloud_arg,
277  bool bShowStatistics_arg)
278  {
279  uint32_t cloud_width;
280  uint32_t cloud_height;
281  float maxDepth;
282  float focalLength;
283  float disparityShift = 0.0f;
284  float disparityScale;
285 
286  // disparity and rgb image data
287  std::vector<uint16_t> disparityData;
288  std::vector<uint8_t> colorData;
289 
290  // compressed disparity and rgb image data
291  std::vector<uint8_t> compressedDisparity;
292  std::vector<uint8_t> compressedColor;
293 
294  uint32_t compressedDisparitySize;
295  uint32_t compressedColorSize;
296 
297  // PNG decoded parameters
298  size_t png_width = 0;
299  size_t png_height = 0;
300  unsigned int png_channels = 1;
301 
302  // sync to frame header
303  unsigned int headerIdPos = 0;
304  bool valid_stream = true;
305  while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
306  {
307  char readChar;
308  compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
309  if (compressedDataIn_arg.gcount()!= sizeof (readChar))
310  valid_stream = false;
311  if (readChar != frameHeaderIdentifier_[headerIdPos++])
312  headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
313 
314  valid_stream &= compressedDataIn_arg.good ();
315  }
316 
317  if (valid_stream) {
318 
319  //////////////
320  // reading frame header
321  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
322  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
323  compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
324  compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
325  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
326  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
327 
328  // reading compressed disparity data
329  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
330  compressedDisparity.resize (compressedDisparitySize);
331  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(uint8_t));
332 
333  // reading compressed rgb data
334  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
335  compressedColor.resize (compressedColorSize);
336  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(uint8_t));
337 
338  // decode PNG compressed disparity data
339  decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
340 
341  // decode PNG compressed rgb data
342  decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
343  }
344 
345  if (disparityShift==0.0f)
346  {
347  // reconstruct point cloud
349  colorData,
350  (png_channels == 1),
351  cloud_width,
352  cloud_height,
353  focalLength,
354  disparityShift,
355  disparityScale,
356  *cloud_arg);
357  } else
358  {
359 
360  // we need to decode a raw shift image
361  std::size_t size = disparityData.size();
362  std::vector<float> depthData;
363  depthData.resize(size);
364 
365  // initialize shift-to-depth converter
366  if (!sd_converter_.isInitialized())
367  sd_converter_.generateLookupTable();
368 
369  // convert shift to depth image
370  for (std::size_t i=0; i<size; ++i)
371  depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
372 
373  // reconstruct point cloud
375  colorData,
376  static_cast<bool>(png_channels==1),
377  cloud_width,
378  cloud_height,
379  focalLength,
380  *cloud_arg);
381  }
382 
383  if (bShowStatistics_arg)
384  {
385  uint64_t pointCount = cloud_width * cloud_height;
386  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
387 
388  PCL_INFO("*** POINTCLOUD DECODING ***\n");
389  PCL_INFO("Number of encoded points: %ld\n", pointCount);
390  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
391  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
392  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
393  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
394  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
395  }
396 
397  return valid_stream;
398  }
399 
400  //////////////////////////////////////////////////////////////////////////////////////////////
401  template<typename PointT> void
403  float& maxDepth_arg,
404  float& focalLength_arg) const
405  {
406  size_t width = cloud_arg->width;
407  size_t height = cloud_arg->height;
408 
409  // Center of organized point cloud
410  int centerX = static_cast<int> (width / 2);
411  int centerY = static_cast<int> (height / 2);
412 
413  // Ensure we have an organized point cloud
414  assert((width>1) && (height>1));
415  assert(width*height == cloud_arg->points.size());
416 
417  float maxDepth = 0;
418  float focalLength = 0;
419 
420  size_t it = 0;
421  for (int y = -centerY; y < centerY; ++y )
422  for (int x = -centerX; x < centerX; ++x )
423  {
424  const PointT& point = cloud_arg->points[it++];
425 
426  if (pcl::isFinite (point))
427  {
428  if (maxDepth < point.z)
429  {
430  // Update maximum depth
431  maxDepth = point.z;
432 
433  // Calculate focal length
434  focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
435  }
436  }
437  }
438 
439  // Update return values
440  maxDepth_arg = maxDepth;
441  focalLength_arg = focalLength;
442  }
443 
444  }
445 }
446 
447 #endif
448 
PCL_EXPORTS void decodePNGToImage(std::vector< uint8_t > &pngData_arg, std::vector< uint8_t > &imageData_arg, size_t &width_arg, size_t &heigh_argt, unsigned int &channels_arg)
Decode compressed PNG to 8-bit image.
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:53
void encodeRawDisparityMapWithColorImage(std::vector< uint16_t > &disparityMap_arg, std::vector< uint8_t > &colorImage_arg, uint32_t width_arg, 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 encodeMonoImageToPNG(std::vector< uint8_t > &image_arg, size_t width_arg, size_t height_arg, std::vector< uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit mono image to PNG format.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
boost::shared_ptr< const PointCloud > PointCloudConstPtr
bool decodePointCloud(std::istream &compressedDataIn_arg, PointCloudPtr &cloud_arg, bool bShowStatistics_arg=true)
Decode point cloud from input stream.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void analyzeOrganizedCloud(PointCloudConstPtr cloud_arg, float &maxDepth_arg, float &focalLength_arg) const
Analyze input point cloud and calculate the maximum depth and focal length.
PCL_EXPORTS void encodeRGBImageToPNG(std::vector< uint8_t > &image_arg, size_t width_arg, size_t height_arg, std::vector< uint8_t > &pngData_arg, int png_level_arg=-1)
Encodes 8-bit RGB image to PNG format.