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.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 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  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;
284  float focalLength;
285  float disparityShift = 0.0f;
286  float disparityScale;
287 
288  // disparity and rgb image data
289  std::vector<uint16_t> disparityData;
290  std::vector<uint8_t> colorData;
291 
292  // compressed disparity and rgb image data
293  std::vector<uint8_t> compressedDisparity;
294  std::vector<uint8_t> compressedColor;
295 
296  uint32_t compressedDisparitySize;
297  uint32_t compressedColorSize;
298 
299  // PNG decoded parameters
300  size_t png_width = 0;
301  size_t png_height = 0;
302  unsigned int png_channels = 1;
303 
304  // sync to frame header
305  unsigned int headerIdPos = 0;
306  bool valid_stream = true;
307  while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
308  {
309  char readChar;
310  compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
311  if (compressedDataIn_arg.gcount()!= sizeof (readChar))
312  valid_stream = false;
313  if (readChar != frameHeaderIdentifier_[headerIdPos++])
314  headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
315 
316  valid_stream &= compressedDataIn_arg.good ();
317  }
318 
319  if (valid_stream) {
320 
321  //////////////
322  // reading frame header
323  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
324  compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
325  compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
326  compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
327  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
328  compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
329 
330  // reading compressed disparity data
331  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
332  compressedDisparity.resize (compressedDisparitySize);
333  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(uint8_t));
334 
335  // reading compressed rgb data
336  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
337  compressedColor.resize (compressedColorSize);
338  compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(uint8_t));
339 
340  // decode PNG compressed disparity data
341  decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
342 
343  // decode PNG compressed rgb data
344  decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
345  }
346 
347  if (disparityShift==0.0f)
348  {
349  // reconstruct point cloud
351  colorData,
352  static_cast<bool>(png_channels==1),
353  cloud_width,
354  cloud_height,
355  focalLength,
356  disparityShift,
357  disparityScale,
358  *cloud_arg);
359  } else
360  {
361 
362  // we need to decode a raw shift image
363  std::size_t size = disparityData.size();
364  std::vector<float> depthData;
365  depthData.resize(size);
366 
367  // initialize shift-to-depth converter
368  if (!sd_converter_.isInitialized())
369  sd_converter_.generateLookupTable();
370 
371  // convert shift to depth image
372  for (std::size_t i=0; i<size; ++i)
373  depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
374 
375  // reconstruct point cloud
377  colorData,
378  static_cast<bool>(png_channels==1),
379  cloud_width,
380  cloud_height,
381  focalLength,
382  *cloud_arg);
383  }
384 
385  if (bShowStatistics_arg)
386  {
387  uint64_t pointCount = cloud_width * cloud_height;
388  float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
389 
390  PCL_INFO("*** POINTCLOUD DECODING ***\n");
391  PCL_INFO("Number of encoded points: %ld\n", pointCount);
392  PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
393  PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
394  PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
395  PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
396  PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
397  }
398 
399  return valid_stream;
400  }
401 
402  //////////////////////////////////////////////////////////////////////////////////////////////
403  template<typename PointT> void
405  float& maxDepth_arg,
406  float& focalLength_arg) const
407  {
408  size_t width, height, it;
409  int centerX, centerY;
410  int x, y;
411  float maxDepth;
412  float focalLength;
413 
414  width = cloud_arg->width;
415  height = cloud_arg->height;
416 
417  // Center of organized point cloud
418  centerX = static_cast<int> (width / 2);
419  centerY = static_cast<int> (height / 2);
420 
421  // Ensure we have an organized point cloud
422  assert((width>1) && (height>1));
423  assert(width*height == cloud_arg->points.size());
424 
425  maxDepth = 0;
426  focalLength = 0;
427 
428  it = 0;
429  for (y = -centerY; y < +centerY; ++y)
430  for (x = -centerX; x < +centerX; ++x)
431  {
432  const PointT& point = cloud_arg->points[it++];
433 
434  if (pcl::isFinite (point))
435  {
436  if (maxDepth < point.z)
437  {
438  // Update maximum depth
439  maxDepth = point.z;
440 
441  // Calculate focal length
442  focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
443  }
444  }
445  }
446 
447  // Update return values
448  maxDepth_arg = maxDepth;
449  focalLength_arg = focalLength;
450  }
451 
452  }
453 }
454 
455 #endif
456 
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.