38 #ifndef ORGANIZED_COMPRESSION_HPP
39 #define ORGANIZED_COMPRESSION_HPP
41 #include <pcl/compression/organized_pointcloud_compression.h>
44 #include <pcl/point_cloud.h>
46 #include <pcl/compression/libpng_wrapper.h>
47 #include <pcl/compression/organized_pointcloud_conversion.h>
57 template<
typename Po
intT>
void
59 std::ostream& compressedDataOut_arg,
62 bool bShowStatistics_arg,
65 std::uint32_t cloud_width = cloud_arg->width;
66 std::uint32_t cloud_height = cloud_arg->height;
68 float maxDepth, focalLength, disparityShift, disparityScale;
71 disparityScale = 1.0f;
72 disparityShift = 0.0f;
74 analyzeOrganizedCloud (cloud_arg, maxDepth, focalLength);
77 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
79 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&cloud_width),
sizeof (cloud_width));
81 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&cloud_height),
sizeof (cloud_height));
83 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&maxDepth),
sizeof (maxDepth));
85 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&focalLength),
sizeof (focalLength));
87 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&disparityScale),
sizeof (disparityScale));
89 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&disparityShift),
sizeof (disparityShift));
92 std::vector<std::uint16_t> disparityData;
93 std::vector<std::uint8_t> colorData;
96 std::vector<std::uint8_t> compressedDisparity;
97 std::vector<std::uint8_t> compressedColor;
99 std::uint32_t compressedDisparitySize = 0;
100 std::uint32_t compressedColorSize = 0;
106 encodeMonoImageToPNG (disparityData, cloud_width, cloud_height, compressedDisparity, pngLevel_arg);
108 compressedDisparitySize =
static_cast<std::uint32_t
>(compressedDisparity.size());
110 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedDisparitySize),
sizeof (compressedDisparitySize));
112 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedDisparity[0]), compressedDisparity.size () *
sizeof(std::uint8_t));
126 compressedColorSize =
static_cast<std::uint32_t
>(compressedColor.size ());
128 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedColorSize),
sizeof (compressedColorSize));
130 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedColor[0]), compressedColor.size () *
sizeof(std::uint8_t));
132 if (bShowStatistics_arg)
134 std::uint64_t pointCount = cloud_width * cloud_height;
135 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) /
static_cast<float> (pointCount);
137 PCL_INFO(
"*** POINTCLOUD ENCODING ***\n");
138 PCL_INFO(
"Number of encoded points: %ld\n", pointCount);
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));
147 compressedDataOut_arg.flush();
151 template<
typename Po
intT>
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,
159 bool bShowStatistics_arg,
161 float focalLength_arg,
162 float disparityShift_arg,
163 float disparityScale_arg)
167 std::size_t cloud_size = width_arg*height_arg;
168 assert (disparityMap_arg.size()==cloud_size);
169 if (!colorImage_arg.empty ())
171 assert (colorImage_arg.size()==cloud_size*3);
175 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (frameHeaderIdentifier_), strlen (frameHeaderIdentifier_));
177 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&width_arg),
sizeof (width_arg));
179 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&height_arg),
sizeof (height_arg));
181 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&maxDepth),
sizeof (maxDepth));
183 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&focalLength_arg),
sizeof (focalLength_arg));
185 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&disparityScale_arg),
sizeof (disparityScale_arg));
187 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&disparityShift_arg),
sizeof (disparityShift_arg));
190 std::vector<std::uint8_t> compressedDisparity;
191 std::vector<std::uint8_t> compressedColor;
193 std::uint32_t compressedDisparitySize = 0;
194 std::uint32_t compressedColorSize = 0;
197 std::uint16_t* depth_ptr = &disparityMap_arg[0];
198 std::uint8_t* color_ptr = &colorImage_arg[0];
200 for (std::size_t i = 0; i < cloud_size; ++i, ++depth_ptr, color_ptr +=
sizeof(std::uint8_t) * 3)
202 if (!(*depth_ptr) || (*depth_ptr==0x7FF))
203 memset(color_ptr, 0,
sizeof(std::uint8_t)*3);
207 encodeMonoImageToPNG (disparityMap_arg, width_arg, height_arg, compressedDisparity, pngLevel_arg);
209 compressedDisparitySize =
static_cast<std::uint32_t
>(compressedDisparity.size());
211 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedDisparitySize),
sizeof (compressedDisparitySize));
213 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedDisparity[0]), compressedDisparity.size () *
sizeof(std::uint8_t));
216 if (!colorImage_arg.empty () && doColorEncoding)
220 std::vector<std::uint8_t> monoImage;
221 std::size_t size = width_arg*height_arg;
223 monoImage.reserve(size);
226 for (std::size_t i = 0; i < size; ++i)
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);
242 compressedColorSize =
static_cast<std::uint32_t
>(compressedColor.size ());
244 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedColorSize),
sizeof (compressedColorSize));
246 compressedDataOut_arg.write (
reinterpret_cast<const char*
> (&compressedColor[0]), compressedColor.size () *
sizeof(std::uint8_t));
248 if (bShowStatistics_arg)
250 std::uint64_t pointCount = width_arg * height_arg;
251 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) /
static_cast<float> (pointCount);
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);
263 compressedDataOut_arg.flush();
267 template<
typename Po
intT>
bool
270 bool bShowStatistics_arg)
272 std::uint32_t cloud_width;
273 std::uint32_t cloud_height;
276 float disparityShift = 0.0f;
277 float disparityScale;
280 std::vector<std::uint16_t> disparityData;
281 std::vector<std::uint8_t> colorData;
284 std::vector<std::uint8_t> compressedDisparity;
285 std::vector<std::uint8_t> compressedColor;
287 std::uint32_t compressedDisparitySize;
288 std::uint32_t compressedColorSize;
291 std::size_t png_width = 0;
292 std::size_t png_height = 0;
293 unsigned int png_channels = 1;
296 unsigned int headerIdPos = 0;
297 bool valid_stream =
true;
298 while (valid_stream && (headerIdPos < strlen (frameHeaderIdentifier_)))
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;
307 valid_stream &= compressedDataIn_arg.good ();
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));
322 compressedDataIn_arg.read (
reinterpret_cast<char*
> (&compressedDisparitySize),
sizeof (compressedDisparitySize));
323 compressedDisparity.resize (compressedDisparitySize);
324 compressedDataIn_arg.read (
reinterpret_cast<char*
> (&compressedDisparity[0]), compressedDisparitySize *
sizeof(std::uint8_t));
327 compressedDataIn_arg.read (
reinterpret_cast<char*
> (&compressedColorSize),
sizeof (compressedColorSize));
328 compressedColor.resize (compressedColorSize);
329 compressedDataIn_arg.read (
reinterpret_cast<char*
> (&compressedColor[0]), compressedColorSize *
sizeof(std::uint8_t));
332 decodePNGToImage (compressedDisparity, disparityData, png_width, png_height, png_channels);
335 decodePNGToImage (compressedColor, colorData, png_width, png_height, png_channels);
338 if (disparityShift==0.0f)
354 std::size_t size = disparityData.size();
355 std::vector<float> depthData;
356 depthData.resize(size);
359 if (!sd_converter_.isInitialized())
360 sd_converter_.generateLookupTable();
363 for (std::size_t i=0; i<size; ++i)
364 depthData[i] = sd_converter_.shiftToDepth(disparityData[i]);
369 static_cast<bool>(png_channels==1),
376 if (bShowStatistics_arg)
378 std::uint64_t pointCount = cloud_width * cloud_height;
379 float bytesPerPoint =
static_cast<float> (compressedDisparitySize+compressedColorSize) /
static_cast<float> (pointCount);
381 PCL_INFO(
"*** POINTCLOUD DECODING ***\n");
382 PCL_INFO(
"Number of encoded points: %ld\n", pointCount);
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));
394 template<
typename Po
intT>
void
397 float& focalLength_arg)
const
399 std::size_t width = cloud_arg->width;
400 std::size_t height = cloud_arg->height;
403 int centerX =
static_cast<int> (width / 2);
404 int centerY =
static_cast<int> (height / 2);
407 assert((width>1) && (height>1));
408 assert(width*height == cloud_arg->size());
411 float focalLength = 0;
414 for (
int y = -centerY; y < centerY; ++y )
415 for (
int x = -centerX; x < centerX; ++x )
417 const PointT& point = (*cloud_arg)[it++];
421 if (maxDepth < point.z)
427 focalLength = 2.0f / (point.x / (
static_cast<float> (x) * point.z) + point.y / (
static_cast<float> (y) * point.z));
433 maxDepth_arg = maxDepth;
434 focalLength_arg = focalLength;