Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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
52namespace 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
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.