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
63 std::uint32_t uncompressed_size;
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;
94 for (std::uint32_t v = 0; v < cloud.
height; ++v)
96 for (std::uint32_t u = 0; u < cloud.
width; ++u, ++point_idx, depth_idx += 2)
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)
129 std::uint32_t uncompressed_size;
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
209 std::uint32_t uncompressed_size;
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
258 std::uint32_t uncompressed_size;
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
314 std::uint32_t uncompressed_size;
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
372 std::uint32_t uncompressed_size;
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
438 std::uint32_t uncompressed_size;
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
488 std::uint32_t uncompressed_size;
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_
double principal_point_x
cx
bool readOMP(const std::string &filename, pcl::PointCloud< PointT > &cloud, unsigned int num_threads=0)
Read the data stored in a PCLZF YUV422 file and convert it to a pcl::PointCloud type.
bool readOMP(const std::string &filename, pcl::PointCloud< PointT > &cloud, unsigned int num_threads=0)
Read the data stored in a PCLZF RGB file and convert it to a pcl::PointCloud type.
void debayerEdgeAware(const unsigned char *bayer_pixel, unsigned char *rgb_buffer, unsigned width, unsigned height, int bayer_line_step=0, int bayer_line_step2=0, unsigned rgb_line_step=0) const
bool read(const std::string &filename, pcl::PointCloud< PointT > &cloud)
Read the data stored in a PCLZF YUV422 16bit file and convert it to a pcl::PointCloud type...
std::uint32_t getHeight() const
Get the image height as read from disk.
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool readOMP(const std::string &filename, pcl::PointCloud< PointT > &cloud, unsigned int num_threads=0)
Read the data stored in a PCLZF Bayer 8bit file and convert it to a pcl::PointCloud type...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
bool read(const std::string &filename, pcl::PointCloud< PointT > &cloud)
Read the data stored in a PCLZF RGB file and convert it to a pcl::PointCloud type.
CameraParameters parameters_
Internal set of camera parameters.
bool readOMP(const std::string &filename, pcl::PointCloud< PointT > &cloud, unsigned int num_threads=0)
Read the data stored in a PCLZF depth file and convert it to a pcl::PointCloud type.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t getWidth() const
Get the image width as read from disk.
double principal_point_y
cy
bool read(const std::string &filename, pcl::PointCloud< PointT > &cloud)
Read the data stored in a PCLZF Bayer 8bit file and convert it to a pcl::PointCloud type...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool read(const std::string &filename, pcl::PointCloud< PointT > &cloud)
Read the data stored in a PCLZF depth file and convert it to a pcl::PointCloud type.
Various debayering methods.
bool loadImageBlob(const std::string &filename, std::vector< char > &data, std::uint32_t &uncompressed_size)
Load a compressed image array from disk.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::string getImageType() const
Get the type of the image read from disk.
double z_multiplication_factor_
Z-value depth multiplication factor (i.e., if raw data is in [mm] and we want [m], we need to multiply with 0.001)
A point structure representing Euclidean xyz coordinates, and the RGB color.
void resize(std::size_t n)
Resize the cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
bool decompress(const std::vector< char > &input, std::vector< char > &output)
Realtime LZF decompression.