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>
44 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
47 template <
typename Po
intT>
bool
51 uint32_t uncompressed_size;
52 std::vector<char> compressed_data;
53 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
55 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
61 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 ());
65 std::vector<char> uncompressed_data (uncompressed_size);
66 decompress (compressed_data, uncompressed_data);
68 if (uncompressed_data.empty ())
70 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
79 int depth_idx = 0, point_idx = 0;
82 for (uint32_t v = 0; v < cloud.
height; ++v)
84 for (uint32_t u = 0; u < cloud.
width; ++u, ++point_idx, depth_idx += 2)
88 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
91 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
98 * pt.z *
static_cast<float> (constant_x);
100 * pt.z *
static_cast<float> (constant_y);
112 template <
typename Po
intT>
bool
115 unsigned int num_threads)
117 uint32_t uncompressed_size;
118 std::vector<char> compressed_data;
119 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
121 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
125 if (uncompressed_size != getWidth () * getHeight () * 2)
127 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 ());
131 std::vector<char> uncompressed_data (uncompressed_size);
132 decompress (compressed_data, uncompressed_data);
134 if (uncompressed_data.empty ())
136 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
141 cloud.
width = getWidth ();
142 cloud.
height = getHeight ();
144 cloud.
resize (getWidth () * getHeight ());
145 double constant_x = 1.0 / parameters_.focal_length_x,
146 constant_y = 1.0 / parameters_.focal_length_y;
148 #pragma omp parallel for num_threads (num_threads)
152 for (
int i = 0; i < static_cast< int> (cloud.
size ()); ++i)
154 int u = i % cloud.
width;
155 int v = i / cloud.
width;
159 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
162 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
176 pt.z =
static_cast<float> (val * z_multiplication_factor_);
177 pt.x = (
static_cast<float> (u) -
static_cast<float> (parameters_.principal_point_x))
178 * pt.z *
static_cast<float> (constant_x);
179 pt.y = (
static_cast<float> (v) -
static_cast<float> (parameters_.principal_point_y))
180 * pt.z *
static_cast<float> (constant_y);
193 template <
typename Po
intT>
bool
197 uint32_t uncompressed_size;
198 std::vector<char> compressed_data;
199 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
201 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
205 if (uncompressed_size != getWidth () * getHeight () * 3)
207 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 ());
211 std::vector<char> uncompressed_data (uncompressed_size);
212 decompress (compressed_data, uncompressed_data);
214 if (uncompressed_data.empty ())
216 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
221 cloud.
width = getWidth ();
222 cloud.
height = getHeight ();
223 cloud.
resize (getWidth () * getHeight ());
226 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
227 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[getWidth () * getHeight ()]);
228 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 * getWidth () * getHeight ()]);
230 for (
size_t i = 0; i < cloud.
size (); ++i, ++rgb_idx)
234 pt.b = color_b[rgb_idx];
235 pt.g = color_g[rgb_idx];
236 pt.r = color_r[rgb_idx];
242 template <
typename Po
intT>
bool
246 uint32_t uncompressed_size;
247 std::vector<char> compressed_data;
248 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
250 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
254 if (uncompressed_size != getWidth () * getHeight () * 3)
256 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 ());
260 std::vector<char> uncompressed_data (uncompressed_size);
261 decompress (compressed_data, uncompressed_data);
263 if (uncompressed_data.empty ())
265 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
270 cloud.
width = getWidth ();
271 cloud.
height = getHeight ();
272 cloud.
resize (getWidth () * getHeight ());
274 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
275 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[getWidth () * getHeight ()]);
276 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 * getWidth () * getHeight ()]);
279 #pragma omp parallel for num_threads (num_threads)
283 for (
long int i = 0; i < cloud.
size (); ++i)
295 template <
typename Po
intT>
bool
299 uint32_t uncompressed_size;
300 std::vector<char> compressed_data;
301 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
303 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
307 if (uncompressed_size != getWidth () * getHeight () * 2)
309 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 ());
313 std::vector<char> uncompressed_data (uncompressed_size);
314 decompress (compressed_data, uncompressed_data);
316 if (uncompressed_data.empty ())
318 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
323 cloud.
width = getWidth ();
324 cloud.
height = getHeight ();
325 cloud.
resize (getWidth () * getHeight ());
327 int wh2 = getWidth () * getHeight () / 2;
328 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
329 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
330 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
333 for (
int i = 0; i < wh2; ++i, y_idx += 2)
335 int v = color_v[i] - 128;
336 int u = color_u[i] - 128;
339 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
340 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
341 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
344 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
345 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
346 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
353 template <
typename Po
intT>
bool
357 uint32_t uncompressed_size;
358 std::vector<char> compressed_data;
359 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
361 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
365 if (uncompressed_size != getWidth () * getHeight () * 2)
367 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 ());
371 std::vector<char> uncompressed_data (uncompressed_size);
372 decompress (compressed_data, uncompressed_data);
374 if (uncompressed_data.empty ())
376 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
381 cloud.
width = getWidth ();
382 cloud.
height = getHeight ();
383 cloud.
resize (getWidth () * getHeight ());
385 int wh2 = getWidth () * getHeight () / 2;
386 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
387 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
388 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
391 #pragma omp parallel for num_threads (num_threads)
395 for (
int i = 0; i < wh2; ++i)
398 int v = color_v[i] - 128;
399 int u = color_u[i] - 128;
402 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
403 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
404 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
407 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
408 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
409 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
416 template <
typename Po
intT>
bool
420 uint32_t uncompressed_size;
421 std::vector<char> compressed_data;
422 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
424 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
428 if (uncompressed_size != getWidth () * getHeight ())
430 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 ());
434 std::vector<char> uncompressed_data (uncompressed_size);
435 decompress (compressed_data, uncompressed_data);
437 if (uncompressed_data.empty ())
439 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
444 std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
446 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
447 static_cast<unsigned char*
> (&rgb_buffer[0]),
448 getWidth (), getHeight ());
450 cloud.
width = getWidth ();
451 cloud.
height = getHeight ();
452 cloud.
resize (getWidth () * getHeight ());
454 for (
size_t i = 0; i < cloud.
size (); ++i, rgb_idx += 3)
458 pt.b = rgb_buffer[rgb_idx + 2];
459 pt.g = rgb_buffer[rgb_idx + 1];
460 pt.r = rgb_buffer[rgb_idx + 0];
466 template <
typename Po
intT>
bool
470 uint32_t uncompressed_size;
471 std::vector<char> compressed_data;
472 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
474 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
478 if (uncompressed_size != getWidth () * getHeight ())
480 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 ());
484 std::vector<char> uncompressed_data (uncompressed_size);
485 decompress (compressed_data, uncompressed_data);
487 if (uncompressed_data.empty ())
489 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
494 std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
496 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
497 static_cast<unsigned char*
> (&rgb_buffer[0]),
498 getWidth (), getHeight ());
500 cloud.
width = getWidth ();
501 cloud.
height = getHeight ();
502 cloud.
resize (getWidth () * getHeight ());
504 #pragma omp parallel for num_threads (num_threads)
508 for (
long int i = 0; i < cloud.
size (); ++i)
511 long int rgb_idx = 3*i;
512 pt.b = rgb_buffer[rgb_idx + 2];
513 pt.g = rgb_buffer[rgb_idx + 1];
514 pt.r = rgb_buffer[rgb_idx + 0];
519 #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_