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))
53 template <
typename Po
intT>
bool
57 std::uint32_t uncompressed_size;
58 std::vector<char> compressed_data;
59 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
61 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
67 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 ());
71 std::vector<char> uncompressed_data (uncompressed_size);
72 decompress (compressed_data, uncompressed_data);
74 if (uncompressed_data.empty ())
76 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
85 int depth_idx = 0, point_idx = 0;
88 for (std::uint32_t v = 0; v < cloud.
height; ++v)
90 for (std::uint32_t u = 0; u < cloud.
width; ++u, ++point_idx, depth_idx += 2)
94 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
97 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
104 * pt.z *
static_cast<float> (constant_x);
106 * pt.z *
static_cast<float> (constant_y);
118 template <
typename Po
intT>
bool
121 unsigned int num_threads)
123 std::uint32_t uncompressed_size;
124 std::vector<char> compressed_data;
125 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
127 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
131 if (uncompressed_size != getWidth () * getHeight () * 2)
133 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 ());
137 std::vector<char> uncompressed_data (uncompressed_size);
138 decompress (compressed_data, uncompressed_data);
140 if (uncompressed_data.empty ())
142 PCL_ERROR (
"[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
147 cloud.
width = getWidth ();
148 cloud.
height = getHeight ();
150 cloud.
resize (getWidth () * getHeight ());
151 double constant_x = 1.0 / parameters_.focal_length_x,
152 constant_y = 1.0 / parameters_.focal_length_y;
154 #pragma omp parallel for num_threads (num_threads)
158 for (
int i = 0; i < static_cast< int> (cloud.
size ()); ++i)
160 int u = i % cloud.
width;
161 int v = i / cloud.
width;
165 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
168 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
182 pt.z =
static_cast<float> (val * z_multiplication_factor_);
183 pt.x = (
static_cast<float> (u) -
static_cast<float> (parameters_.principal_point_x))
184 * pt.z *
static_cast<float> (constant_x);
185 pt.y = (
static_cast<float> (v) -
static_cast<float> (parameters_.principal_point_y))
186 * pt.z *
static_cast<float> (constant_y);
199 template <
typename Po
intT>
bool
203 std::uint32_t uncompressed_size;
204 std::vector<char> compressed_data;
205 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
207 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
211 if (uncompressed_size != getWidth () * getHeight () * 3)
213 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 ());
217 std::vector<char> uncompressed_data (uncompressed_size);
218 decompress (compressed_data, uncompressed_data);
220 if (uncompressed_data.empty ())
222 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
227 cloud.
width = getWidth ();
228 cloud.
height = getHeight ();
229 cloud.
resize (getWidth () * getHeight ());
232 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
233 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[getWidth () * getHeight ()]);
234 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 * getWidth () * getHeight ()]);
236 for (std::size_t i = 0; i < cloud.
size (); ++i, ++rgb_idx)
240 pt.b = color_b[rgb_idx];
241 pt.g = color_g[rgb_idx];
242 pt.r = color_r[rgb_idx];
248 template <
typename Po
intT>
bool
252 std::uint32_t uncompressed_size;
253 std::vector<char> compressed_data;
254 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
256 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
260 if (uncompressed_size != getWidth () * getHeight () * 3)
262 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 ());
266 std::vector<char> uncompressed_data (uncompressed_size);
267 decompress (compressed_data, uncompressed_data);
269 if (uncompressed_data.empty ())
271 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
276 cloud.
width = getWidth ();
277 cloud.
height = getHeight ();
278 cloud.
resize (getWidth () * getHeight ());
280 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
281 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[getWidth () * getHeight ()]);
282 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 * getWidth () * getHeight ()]);
285 #pragma omp parallel for num_threads (num_threads)
289 for (
long int i = 0; i < cloud.
size (); ++i)
301 template <
typename Po
intT>
bool
305 std::uint32_t uncompressed_size;
306 std::vector<char> compressed_data;
307 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
309 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
313 if (uncompressed_size != getWidth () * getHeight () * 2)
315 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 ());
319 std::vector<char> uncompressed_data (uncompressed_size);
320 decompress (compressed_data, uncompressed_data);
322 if (uncompressed_data.empty ())
324 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
329 cloud.
width = getWidth ();
330 cloud.
height = getHeight ();
331 cloud.
resize (getWidth () * getHeight ());
333 int wh2 = getWidth () * getHeight () / 2;
334 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
335 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
336 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
339 for (
int i = 0; i < wh2; ++i, y_idx += 2)
341 int v = color_v[i] - 128;
342 int u = color_u[i] - 128;
345 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
346 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
347 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
350 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
351 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
352 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
359 template <
typename Po
intT>
bool
363 std::uint32_t uncompressed_size;
364 std::vector<char> compressed_data;
365 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
367 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
371 if (uncompressed_size != getWidth () * getHeight () * 2)
373 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 ());
377 std::vector<char> uncompressed_data (uncompressed_size);
378 decompress (compressed_data, uncompressed_data);
380 if (uncompressed_data.empty ())
382 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
387 cloud.
width = getWidth ();
388 cloud.
height = getHeight ();
389 cloud.
resize (getWidth () * getHeight ());
391 int wh2 = getWidth () * getHeight () / 2;
392 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
393 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
394 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);
397 #pragma omp parallel for num_threads (num_threads)
401 for (
int i = 0; i < wh2; ++i)
404 int v = color_v[i] - 128;
405 int u = color_u[i] - 128;
408 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
409 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
410 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
413 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
414 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
415 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
422 template <
typename Po
intT>
bool
426 std::uint32_t uncompressed_size;
427 std::vector<char> compressed_data;
428 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
430 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
434 if (uncompressed_size != getWidth () * getHeight ())
436 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 ());
440 std::vector<char> uncompressed_data (uncompressed_size);
441 decompress (compressed_data, uncompressed_data);
443 if (uncompressed_data.empty ())
445 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
450 std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
452 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
453 static_cast<unsigned char*
> (&rgb_buffer[0]),
454 getWidth (), getHeight ());
456 cloud.
width = getWidth ();
457 cloud.
height = getHeight ();
458 cloud.
resize (getWidth () * getHeight ());
460 for (std::size_t i = 0; i < cloud.
size (); ++i, rgb_idx += 3)
464 pt.b = rgb_buffer[rgb_idx + 2];
465 pt.g = rgb_buffer[rgb_idx + 1];
466 pt.r = rgb_buffer[rgb_idx + 0];
472 template <
typename Po
intT>
bool
476 std::uint32_t uncompressed_size;
477 std::vector<char> compressed_data;
478 if (!loadImageBlob (filename, compressed_data, uncompressed_size))
480 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
484 if (uncompressed_size != getWidth () * getHeight ())
486 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 ());
490 std::vector<char> uncompressed_data (uncompressed_size);
491 decompress (compressed_data, uncompressed_data);
493 if (uncompressed_data.empty ())
495 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
500 std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
502 i.
debayerEdgeAware (
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]),
503 static_cast<unsigned char*
> (&rgb_buffer[0]),
504 getWidth (), getHeight ());
506 cloud.
width = getWidth ();
507 cloud.
height = getHeight ();
508 cloud.
resize (getWidth () * getHeight ());
510 #pragma omp parallel for num_threads (num_threads)
514 for (
long int i = 0; i < cloud.
size (); ++i)
517 long int rgb_idx = 3*i;
518 pt.b = rgb_buffer[rgb_idx + 2];
519 pt.g = rgb_buffer[rgb_idx + 1];
520 pt.r = rgb_buffer[rgb_idx + 0];
525 #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_