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 ());
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 ());
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 ();
178 * pt.z * static_cast<float> (constant_x);
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 ());
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 ());
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 ());
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 ());
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 ());
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 ());
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 ());
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 ());
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 ());
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 ());
446 i.
debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
447 static_cast<unsigned char*> (&rgb_buffer[0]),
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 ());
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 ());
496 i.
debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
497 static_cast<unsigned char*> (&rgb_buffer[0]),
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_ double principal_point_x
cx
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
uint32_t getWidth() const
Get the image width as read from disk.
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...
uint32_t height
The point cloud height (if organized as an image-structure).
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.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
uint32_t width
The point cloud width (if organized as an image-structure).
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.
double principal_point_y
cy
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
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...
std::string getImageType() const
Get the type of the image read from disk.
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 is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
bool loadImageBlob(const std::string &filename, std::vector< char > &data, uint32_t &uncompressed_size)
Load a compressed image array from disk.
void resize(size_t n)
Resize the cloud.
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.
uint32_t getHeight() const
Get the image height as read from disk.
bool decompress(const std::vector< char > &input, std::vector< char > &output)
Realtime LZF decompression.