38 #ifndef ORGANIZED_COMPRESSION_HPP
39 #define ORGANIZED_COMPRESSION_HPP
41 #include <pcl/compression/organized_pointcloud_compression.h>
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_cloud.h>
46 #include <pcl/common/boost.h>
47 #include <pcl/common/eigen.h>
48 #include <pcl/common/common.h>
49 #include <pcl/common/io.h>
51 #include <pcl/compression/libpng_wrapper.h>
52 #include <pcl/compression/organized_pointcloud_conversion.h>
64 template<
typename Po
intT>
void
66 std::ostream& compressedDataOut_arg,
69 bool bShowStatistics_arg,
72 uint32_t cloud_width = cloud_arg->width;
73 uint32_t cloud_height = cloud_arg->height;
75 float maxDepth, focalLength, disparityShift, disparityScale;
78 disparityScale = 1.0f;
79 disparityShift = 0.0f;
81 analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength);
84 compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
86 compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_width),
sizeof (cloud_width));
88 compressedDataOut_arg.write (reinterpret_cast<const char*> (&cloud_height),
sizeof (cloud_height));
90 compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth),
sizeof (maxDepth));
92 compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength),
sizeof (focalLength));
94 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale),
sizeof (disparityScale));
96 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift),
sizeof (disparityShift));
99 std::vector<uint16_t> disparityData;
100 std::vector<uint8_t> colorData;
103 std::vector<uint8_t> compressedDisparity;
104 std::vector<uint8_t> compressedColor;
106 uint32_t compressedDisparitySize = 0;
107 uint32_t compressedColorSize = 0;
113 encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg);
115 compressedDisparitySize =
static_cast<uint32_t
>(compressedDisparity.size());
117 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize),
sizeof (compressedDisparitySize));
119 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () *
sizeof(uint8_t));
133 compressedColorSize =
static_cast<uint32_t
>(compressedColor.size ());
135 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize),
sizeof (compressedColorSize));
137 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () *
sizeof(uint8_t));
139 if (bShowStatistics_arg)
141 uint64_t pointCount = cloud_width * cloud_height;
142 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
144 PCL_INFO(
"*** POINTCLOUD ENCODING ***\n");
145 PCL_INFO(
"Number of encoded points: %ld\n", pointCount);
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);
154 compressedDataOut_arg.flush();
158 template<
typename Po
intT>
void
160 std::vector<uint8_t>& colorImage_arg,
163 std::ostream& compressedDataOut_arg,
164 bool doColorEncoding,
166 bool bShowStatistics_arg,
168 float focalLength_arg,
169 float disparityShift_arg,
170 float disparityScale_arg)
174 size_t cloud_size = width_arg*height_arg;
175 assert (disparityMap_arg.size()==cloud_size);
176 if (colorImage_arg.size())
178 assert (colorImage_arg.size()==cloud_size*3);
182 compressedDataOut_arg.write (reinterpret_cast<const char*> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
184 compressedDataOut_arg.write (reinterpret_cast<const char*> (&width_arg),
sizeof (width_arg));
186 compressedDataOut_arg.write (reinterpret_cast<const char*> (&height_arg),
sizeof (height_arg));
188 compressedDataOut_arg.write (reinterpret_cast<const char*> (&maxDepth),
sizeof (maxDepth));
190 compressedDataOut_arg.write (reinterpret_cast<const char*> (&focalLength_arg),
sizeof (focalLength_arg));
192 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityScale_arg),
sizeof (disparityScale_arg));
194 compressedDataOut_arg.write (reinterpret_cast<const char*> (&disparityShift_arg),
sizeof (disparityShift_arg));
197 std::vector<uint8_t> compressedDisparity;
198 std::vector<uint8_t> compressedColor;
200 uint32_t compressedDisparitySize = 0;
201 uint32_t compressedColorSize = 0;
204 uint16_t* depth_ptr = &disparityMap_arg[0];
205 uint8_t* color_ptr = &colorImage_arg[0];
208 for (i=0; i<cloud_size; ++i, ++depth_ptr, color_ptr+=
sizeof(uint8_t)*3)
210 if (!(*depth_ptr) || (*depth_ptr==0x7FF))
211 memset(color_ptr, 0,
sizeof(uint8_t)*3);
215 encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
217 compressedDisparitySize =
static_cast<uint32_t
>(compressedDisparity.size());
219 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparitySize),
sizeof (compressedDisparitySize));
221 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedDisparity[0]), compressedDisparity.size () *
sizeof(uint8_t));
224 if (colorImage_arg.size() && doColorEncoding)
229 vector<uint8_t> monoImage;
230 size = width_arg*height_arg;
232 monoImage.reserve(size);
235 for (i=0; i<size; ++i)
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);
251 compressedColorSize =
static_cast<uint32_t
>(compressedColor.size ());
253 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColorSize),
sizeof (compressedColorSize));
255 compressedDataOut_arg.write (reinterpret_cast<const char*> (&compressedColor[0]), compressedColor.size () *
sizeof(uint8_t));
257 if (bShowStatistics_arg)
259 uint64_t pointCount = width_arg * height_arg;
260 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
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);
272 compressedDataOut_arg.flush();
276 template<
typename Po
intT>
bool
279 bool bShowStatistics_arg)
281 uint32_t cloud_width;
282 uint32_t cloud_height;
283 float maxDepth, focalLength, disparityShift, disparityScale;
286 std::vector<uint16_t> disparityData;
287 std::vector<uint8_t> colorData;
290 std::vector<uint8_t> compressedDisparity;
291 std::vector<uint8_t> compressedColor;
293 uint32_t compressedDisparitySize;
294 uint32_t compressedColorSize;
297 size_t png_width = 0;
298 size_t png_height = 0;
299 unsigned int png_channels = 1;
302 unsigned int headerIdPos = 0;
303 bool valid_stream =
true;
304 while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
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;
313 valid_stream &= compressedDataIn_arg.good ();
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));
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));
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));
338 decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
341 decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
344 if (disparityShift==0.0f)
349 static_cast<bool>(png_channels==1),
360 std::size_t size = disparityData.size();
361 std::vector<float> depthData;
362 depthData.resize(size);
365 if (!sd_converter_.isInitialized())
366 sd_converter_.generateLookupTable();
369 for (std::size_t i=0; i<size; ++i)
370 depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
375 static_cast<bool>(png_channels==1),
382 if (bShowStatistics_arg)
384 uint64_t pointCount = cloud_width * cloud_height;
385 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) / static_cast<float> (pointCount);
387 PCL_INFO(
"*** POINTCLOUD DECODING ***\n");
388 PCL_INFO(
"Number of encoded points: %ld\n", pointCount);
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);
400 template<
typename Po
intT>
void
403 float& focalLength_arg)
const
405 size_t width, height, it;
406 int centerX, centerY;
411 width = cloud_arg->width;
412 height = cloud_arg->height;
415 centerX =
static_cast<int> (width / 2);
416 centerY =
static_cast<int> (height / 2);
419 assert((width>1) && (height>1));
420 assert(width*height == cloud_arg->points.size());
426 for (y = -centerY; y < +centerY; ++y)
427 for (x = -centerX; x < +centerX; ++x)
429 const PointT& point = cloud_arg->points[it++];
433 if (maxDepth < point.z)
439 focalLength = 2.0f / (point.x / (
static_cast<float> (x) * point.z) + point.y / (
static_cast<float> (y) * point.z));
445 maxDepth_arg = maxDepth;
446 focalLength_arg = focalLength;
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< 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.
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.
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.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
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.
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.
boost::shared_ptr< PointCloud > PointCloudPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.