38 #ifndef PCL_LZF_IMAGE_IO_HPP_
39 #define PCL_LZF_IMAGE_IO_HPP_
41 #include <pcl/console/print.h>
42 #include <pcl/io/debayer.h>
50 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
59 template <
typename Po
intT>
bool
64 std::vector<char> compressed_data;
65 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
67 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
73 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 ());
77 std::vector<char> uncompressed_data (uncompressed_size);
78 decompress (compressed_data, uncompressed_data);
80 if (uncompressed_data.empty ())
82 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
91 int depth_idx = 0, point_idx = 0;
100 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
103 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
110 * pt.z *
static_cast<float> (constant_x);
112 * pt.z *
static_cast<float> (constant_y);
124 template <
typename Po
intT>
bool
127 unsigned int num_threads)
130 std::vector<char> compressed_data;
131 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
133 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
139 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 ());
143 std::vector<char> uncompressed_data (uncompressed_size);
144 decompress (compressed_data, uncompressed_data);
146 if (uncompressed_data.empty ())
148 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
160 #pragma omp parallel for \
162 shared(cloud, constant_x, constant_y, uncompressed_data) \
163 num_threads(num_threads)
167 for (
int i = 0; i < static_cast< int> (cloud.
size ()); ++i)
169 int u = i % cloud.
width;
170 int v = i / cloud.
width;
174 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
177 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
191 * pt.z *
static_cast<float> (constant_x);
193 * pt.z *
static_cast<float> (constant_y);
205 template <
typename Po
intT>
bool
210 std::vector<char> compressed_data;
211 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
213 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
219 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 ());
223 std::vector<char> uncompressed_data (uncompressed_size);
224 decompress (compressed_data, uncompressed_data);
226 if (uncompressed_data.empty ())
228 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
238 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
239 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
240 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
242 for (std::size_t i = 0; i < cloud.
size (); ++i, ++rgb_idx)
246 pt.b = color_b[rgb_idx];
247 pt.g = color_g[rgb_idx];
248 pt.r = color_r[rgb_idx];
254 template <
typename Po
intT>
bool
259 std::vector<char> compressed_data;
260 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
262 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
268 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 ());
272 std::vector<char> uncompressed_data (uncompressed_size);
273 decompress (compressed_data, uncompressed_data);
275 if (uncompressed_data.empty ())
277 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
286 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
287 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
288 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
291 #pragma omp parallel for \
293 shared(cloud, color_b, color_g, color_r) \
294 num_threads(num_threads)
298 for (
long int i = 0; i < cloud.
size (); ++i)
310 template <
typename Po
intT>
bool
315 std::vector<char> compressed_data;
316 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
318 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
324 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 ());
328 std::vector<char> uncompressed_data (uncompressed_size);
329 decompress (compressed_data, uncompressed_data);
331 if (uncompressed_data.empty ())
333 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
343 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
344 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
345 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
348 for (
int i = 0; i < wh2; ++i, y_idx += 2)
350 int v = color_v[i] - 128;
351 int u = color_u[i] - 128;
354 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
355 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
356 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
359 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
360 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
361 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
368 template <
typename Po
intT>
bool
373 std::vector<char> compressed_data;
374 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
376 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
382 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 ());
386 std::vector<char> uncompressed_data (uncompressed_size);
387 decompress (compressed_data, uncompressed_data);
389 if (uncompressed_data.empty ())
391 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
401 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
402 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
403 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
406 #pragma omp parallel for \
408 shared(cloud, color_u, color_v, color_y, wh2) \
409 num_threads(num_threads)
413 for (
int i = 0; i < wh2; ++i)
416 int v = color_v[i] - 128;
417 int u = color_u[i] - 128;
420 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
421 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
422 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
425 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
426 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
427 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
434 template <
typename Po
intT>
bool
439 std::vector<char> compressed_data;
440 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
442 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
448 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 ());
452 std::vector<char> uncompressed_data (uncompressed_size);
453 decompress (compressed_data, uncompressed_data);
455 if (uncompressed_data.empty ())
457 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
464 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
465 static_cast<unsigned char*
> (&rgb_buffer[0]),
472 for (std::size_t i = 0; i < cloud.
size (); ++i, rgb_idx += 3)
476 pt.b = rgb_buffer[rgb_idx + 2];
477 pt.g = rgb_buffer[rgb_idx + 1];
478 pt.r = rgb_buffer[rgb_idx + 0];
484 template <
typename Po
intT>
bool
489 std::vector<char> compressed_data;
490 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
492 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
498 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 ());
502 std::vector<char> uncompressed_data (uncompressed_size);
503 decompress (compressed_data, uncompressed_data);
505 if (uncompressed_data.empty ())
507 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
514 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
515 static_cast<unsigned char*
> (&rgb_buffer[0]),
522 #pragma omp parallel for \
524 num_threads(num_threads)
528 for (
long int i = 0; i < cloud.
size (); ++i)
531 long int rgb_idx = 3*i;
532 pt.b = rgb_buffer[rgb_idx + 2];
533 pt.g = rgb_buffer[rgb_idx + 1];
534 pt.r = rgb_buffer[rgb_idx + 0];
542 #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_