38 #ifndef PCL_LZF_IMAGE_IO_HPP_
39 #define PCL_LZF_IMAGE_IO_HPP_
41 #include <pcl/console/print.h>
42 #include <pcl/common/utils.h>
43 #include <pcl/io/debayer.h>
51 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
60 template <
typename Po
intT>
bool
64 std::uint32_t uncompressed_size;
65 std::vector<char> compressed_data;
66 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
68 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
74 PCL_DEBUG (
"[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight () * 2, filename.c_str (),
getImageType ().c_str ());
78 std::vector<char> uncompressed_data (uncompressed_size);
79 decompress (compressed_data, uncompressed_data);
81 if (uncompressed_data.empty ())
83 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
92 int depth_idx = 0, point_idx = 0;
95 for (std::uint32_t v = 0; v < cloud.
height; ++v)
97 for (std::uint32_t u = 0; u < cloud.
width; ++u, ++point_idx, depth_idx += 2)
99 PointT &pt = cloud[point_idx];
101 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
104 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
111 * pt.z *
static_cast<float> (constant_x);
113 * pt.z *
static_cast<float> (constant_y);
125 template <
typename Po
intT>
bool
128 unsigned int num_threads)
130 std::uint32_t uncompressed_size;
131 std::vector<char> compressed_data;
132 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
134 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
140 PCL_DEBUG (
"[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight () * 2, filename.c_str (),
getImageType ().c_str ());
144 std::vector<char> uncompressed_data (uncompressed_size);
145 decompress (compressed_data, uncompressed_data);
147 if (uncompressed_data.empty ())
149 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
161 #pragma omp parallel for \
163 shared(cloud, constant_x, constant_y, uncompressed_data) \
164 num_threads(num_threads)
168 for (
int i = 0; i < static_cast< int> (cloud.
size ()); ++i)
170 int u = i % cloud.
width;
171 int v = i / cloud.
width;
175 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
178 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
192 * pt.z *
static_cast<float> (constant_x);
194 * pt.z *
static_cast<float> (constant_y);
206 template <
typename Po
intT>
bool
210 std::uint32_t uncompressed_size;
211 std::vector<char> compressed_data;
212 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
214 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
220 PCL_DEBUG (
"[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight () * 3, filename.c_str (),
getImageType ().c_str ());
224 std::vector<char> uncompressed_data (uncompressed_size);
225 decompress (compressed_data, uncompressed_data);
227 if (uncompressed_data.empty ())
229 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
239 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
240 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
241 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
243 for (std::size_t i = 0; i < cloud.
size (); ++i, ++rgb_idx)
247 pt.b = color_b[rgb_idx];
248 pt.g = color_g[rgb_idx];
249 pt.r = color_r[rgb_idx];
255 template <
typename Po
intT>
bool
259 std::uint32_t uncompressed_size;
260 std::vector<char> compressed_data;
261 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
263 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
269 PCL_DEBUG (
"[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight () * 3, filename.c_str (),
getImageType ().c_str ());
273 std::vector<char> uncompressed_data (uncompressed_size);
274 decompress (compressed_data, uncompressed_data);
276 if (uncompressed_data.empty ())
278 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
287 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
288 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
289 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
292 #pragma omp parallel for \
294 shared(cloud, color_b, color_g, color_r) \
295 num_threads(num_threads)
299 for (
long int i = 0; i < cloud.
size (); ++i)
311 template <
typename Po
intT>
bool
315 std::uint32_t uncompressed_size;
316 std::vector<char> compressed_data;
317 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
319 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
325 PCL_DEBUG (
"[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight (), filename.c_str (),
getImageType ().c_str ());
329 std::vector<char> uncompressed_data (uncompressed_size);
330 decompress (compressed_data, uncompressed_data);
332 if (uncompressed_data.empty ())
334 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
344 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
345 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
346 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
349 for (
int i = 0; i < wh2; ++i, y_idx += 2)
351 int v = color_v[i] - 128;
352 int u = color_u[i] - 128;
354 PointT &pt1 = cloud[y_idx + 0];
355 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
356 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
357 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
359 PointT &pt2 = cloud[y_idx + 1];
360 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
361 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
362 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
369 template <
typename Po
intT>
bool
373 std::uint32_t uncompressed_size;
374 std::vector<char> compressed_data;
375 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
377 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
383 PCL_DEBUG (
"[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight (), filename.c_str (),
getImageType ().c_str ());
387 std::vector<char> uncompressed_data (uncompressed_size);
388 decompress (compressed_data, uncompressed_data);
390 if (uncompressed_data.empty ())
392 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
402 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
403 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
404 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
407 #pragma omp parallel for \
409 shared(cloud, color_u, color_v, color_y, wh2) \
410 num_threads(num_threads)
414 for (
int i = 0; i < wh2; ++i)
417 int v = color_v[i] - 128;
418 int u = color_u[i] - 128;
420 PointT &pt1 = cloud[y_idx + 0];
421 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
422 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
423 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
425 PointT &pt2 = cloud[y_idx + 1];
426 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
427 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
428 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
435 template <
typename Po
intT>
bool
439 std::uint32_t uncompressed_size;
440 std::vector<char> compressed_data;
441 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
443 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
449 PCL_DEBUG (
"[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight (), filename.c_str (),
getImageType ().c_str ());
453 std::vector<char> uncompressed_data (uncompressed_size);
454 decompress (compressed_data, uncompressed_data);
456 if (uncompressed_data.empty ())
458 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
465 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
466 static_cast<unsigned char*
> (&rgb_buffer[0]),
473 for (std::size_t i = 0; i < cloud.
size (); ++i, rgb_idx += 3)
477 pt.b = rgb_buffer[rgb_idx + 2];
478 pt.g = rgb_buffer[rgb_idx + 1];
479 pt.r = rgb_buffer[rgb_idx + 0];
485 template <
typename Po
intT>
bool
489 std::uint32_t uncompressed_size;
490 std::vector<char> compressed_data;
491 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
493 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
499 PCL_DEBUG (
"[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size,
getWidth () *
getHeight (), filename.c_str (),
getImageType ().c_str ());
503 std::vector<char> uncompressed_data (uncompressed_size);
504 decompress (compressed_data, uncompressed_data);
506 if (uncompressed_data.empty ())
508 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
515 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
516 static_cast<unsigned char*
> (&rgb_buffer[0]),
523 #pragma omp parallel for \
525 num_threads(num_threads)
529 for (
long int i = 0; i < cloud.
size (); ++i)
532 long int rgb_idx = 3*i;
533 pt.b = rgb_buffer[rgb_idx + 2];
534 pt.g = rgb_buffer[rgb_idx + 1];
535 pt.r = rgb_buffer[rgb_idx + 0];
543 #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_