Point Cloud Library (PCL) 1.12.0
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,
63 int pngLevel_arg)
64 {
65 std::uint32_t cloud_width = cloud_arg->width;
66 std::uint32_t cloud_height = cloud_arg->height;
67
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
104
105 // Compress disparity information
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[0]), compressedDisparity.size () * sizeof(std::uint8_t));
113
114 // Compress color information
115 if (CompressionPointTraits<PointT>::hasColor && doColorEncoding)
116 {
117 if (convertToMono)
118 {
120 } else
121 {
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[0]), compressedColor.size () * sizeof(std::uint8_t));
131
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,
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[0];
198 std::uint8_t* color_ptr = &colorImage_arg[0];
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 memset(color_ptr, 0, sizeof(std::uint8_t)*3);
204 }
205
206 // Compress disparity information
208
209 compressedDisparitySize = static_cast<std::uint32_t>(compressedDisparity.size());
210 // Encode size of compressed disparity image data
211 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
212 // Output compressed disparity to ostream
213 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () * sizeof(std::uint8_t));
214
215 // Compress color information
216 if (!colorImage_arg.empty () && doColorEncoding)
217 {
218 if (convertToMono)
219 {
220 std::vector<std::uint8_t> monoImage;
221 std::size_t size = width_arg*height_arg;
222
223 monoImage.reserve(size);
224
225 // grayscale conversion
226 for (std::size_t i = 0; i < size; ++i)
227 {
228 std::uint8_t grayvalue = static_cast<std::uint8_t>(0.2989 * static_cast<float>(colorImage_arg[i*3+0]) +
229 0.5870 * static_cast<float>(colorImage_arg[i*3+1]) +
230 0.1140 * static_cast<float>(colorImage_arg[i*3+2]));
231 monoImage.push_back(grayvalue);
232 }
234
235 } else
236 {
238 }
239
240 }
241
242 compressedColorSize = static_cast<std::uint32_t>(compressedColor.size ());
243 // Encode size of compressed Color image data
244 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize), sizeof (compressedColorSize));
245 // Output compressed disparity to ostream
246 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () * sizeof(std::uint8_t));
247
249 {
250 std::uint64_t pointCount = width_arg * height_arg;
251 float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
252
253 PCL_INFO("*** POINTCLOUD ENCODING ***\n");
254 PCL_INFO("Number of encoded points: %ld\n", pointCount);
255 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);
256 PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
257 PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
258 PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (sizeof(std::uint8_t)*3+sizeof(std::uint16_t)) * 100.0f);
259 PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
260 }
261
262 // flush output stream
263 compressedDataOut_arg.flush();
264 }
265
266 //////////////////////////////////////////////////////////////////////////////////////////////
267 template<typename PointT> bool
271 {
272 std::uint32_t cloud_width;
273 std::uint32_t cloud_height;
274 float maxDepth;
275 float focalLength;
276 float disparityShift = 0.0f;
277 float disparityScale;
278
279 // disparity and rgb image data
280 std::vector<std::uint16_t> disparityData;
281 std::vector<std::uint8_t> colorData;
282
283 // compressed disparity and rgb image data
284 std::vector<std::uint8_t> compressedDisparity;
285 std::vector<std::uint8_t> compressedColor;
286
287 std::uint32_t compressedDisparitySize;
288 std::uint32_t compressedColorSize;
289
290 // PNG decoded parameters
291 std::size_t png_width = 0;
292 std::size_t png_height = 0;
293 unsigned int png_channels = 1;
294
295 // sync to frame header
296 unsigned int headerIdPos = 0;
297 bool valid_stream = true;
298 while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
299 {
300 char readChar;
301 compressedDataIn_arg.read (static_cast<char*> (&readChar), sizeof (readChar));
302 if (compressedDataIn_arg.gcount()!= sizeof (readChar))
303 valid_stream = false;
304 if (readChar != frameHeaderIdentifier_[headerIdPos++])
305 headerIdPos = (frameHeaderIdentifier_[0] == readChar) ? 1 : 0;
306
308 }
309
310 if (valid_stream) {
311
312 //////////////
313 // reading frame header
314 compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_width), sizeof (cloud_width));
315 compressedDataIn_arg.read (reinterpret_cast<char*> (&cloud_height), sizeof (cloud_height));
316 compressedDataIn_arg.read (reinterpret_cast<char*> (&maxDepth), sizeof (maxDepth));
317 compressedDataIn_arg.read (reinterpret_cast<char*> (&focalLength), sizeof (focalLength));
318 compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityScale), sizeof (disparityScale));
319 compressedDataIn_arg.read (reinterpret_cast<char*> (&disparityShift), sizeof (disparityShift));
320
321 // reading compressed disparity data
322 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparitySize), sizeof (compressedDisparitySize));
324 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedDisparity[0]), compressedDisparitySize * sizeof(std::uint8_t));
325
326 // reading compressed rgb data
327 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColorSize), sizeof (compressedColorSize));
329 compressedDataIn_arg.read (reinterpret_cast<char*> (&compressedColor[0]), compressedColorSize * sizeof(std::uint8_t));
330
331 // decode PNG compressed disparity data
333
334 // decode PNG compressed rgb data
336 }
337
338 if (disparityShift==0.0f)
339 {
340 // reconstruct point cloud
342 colorData,
343 (png_channels == 1),
349 *cloud_arg);
350 } else
351 {
352
353 // we need to decode a raw shift image
354 std::size_t size = disparityData.size();
355 std::vector<float> depthData;
356 depthData.resize(size);
357
358 // initialize shift-to-depth converter
359 if (!sd_converter_.isInitialized())
360 sd_converter_.generateLookupTable();
361
362 // convert shift to depth image
363 for (std::size_t i=0; i<size; ++i)
364 depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
365
366 // reconstruct point cloud
368 colorData,
369 static_cast<bool>(png_channels==1),
373 *cloud_arg);
374 }
375
377 {
378 std::uint64_t pointCount = cloud_width * cloud_height;
379 float bytesPerPoint = static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
380
381 PCL_INFO("*** POINTCLOUD DECODING ***\n");
382 PCL_INFO("Number of encoded points: %ld\n", pointCount);
383 PCL_INFO("Size of uncompressed point cloud: %.2f kBytes\n", (static_cast<float> (pointCount) * CompressionPointTraits<PointT>::bytesPerPoint) / 1024.0f);
384 PCL_INFO("Size of compressed point cloud: %.2f kBytes\n", static_cast<float> (compressedDisparitySize+compressedColorSize) / 1024.0f);
385 PCL_INFO("Total bytes per point: %.4f bytes\n", static_cast<float> (bytesPerPoint));
386 PCL_INFO("Total compression percentage: %.4f%%\n", (bytesPerPoint) / (CompressionPointTraits<PointT>::bytesPerPoint) * 100.0f);
387 PCL_INFO("Compression ratio: %.2f\n\n", static_cast<float> (CompressionPointTraits<PointT>::bytesPerPoint) / bytesPerPoint);
388 }
389
390 return valid_stream;
391 }
392
393 //////////////////////////////////////////////////////////////////////////////////////////////
394 template<typename PointT> void
396 float& maxDepth_arg,
397 float& focalLength_arg) const
398 {
399 std::size_t width = cloud_arg->width;
400 std::size_t height = cloud_arg->height;
401
402 // Center of organized point cloud
403 int centerX = static_cast<int> (width / 2);
404 int centerY = static_cast<int> (height / 2);
405
406 // Ensure we have an organized point cloud
407 assert((width>1) && (height>1));
408 assert(width*height == cloud_arg->size());
409
410 float maxDepth = 0;
411 float focalLength = 0;
412
413 std::size_t it = 0;
414 for (int y = -centerY; y < centerY; ++y )
415 for (int x = -centerX; x < centerX; ++x )
416 {
417 const PointT& point = (*cloud_arg)[it++];
418
419 if (pcl::isFinite (point))
420 {
421 if (maxDepth < point.z)
422 {
423 // Update maximum depth
424 maxDepth = point.z;
425
426 // Calculate focal length
427 focalLength = 2.0f / (point.x / (static_cast<float> (x) * point.z) + point.y / (static_cast<float> (y) * point.z));
428 }
429 }
430 }
431
432 // Update return values
433 maxDepth_arg = maxDepth;
435 }
436
437 }
438}
439
440#endif
441
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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.