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) 150 for (
int i = 0; i < static_cast< int> (cloud.
size ()); ++i)
152 int u = i % cloud.
width;
153 int v = i / cloud.
width;
157 memcpy (&val, &uncompressed_data[depth_idx],
sizeof (
unsigned short));
160 pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
176 * pt.z * static_cast<float> (constant_x);
178 * pt.z * static_cast<float> (constant_y);
191 template <
typename Po
intT>
bool 195 uint32_t uncompressed_size;
196 std::vector<char> compressed_data;
197 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
199 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
205 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 ());
209 std::vector<char> uncompressed_data (uncompressed_size);
210 decompress (compressed_data, uncompressed_data);
212 if (uncompressed_data.empty ())
214 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
224 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
225 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
226 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
228 for (
size_t i = 0; i < cloud.
size (); ++i, ++rgb_idx)
232 pt.b = color_b[rgb_idx];
233 pt.g = color_g[rgb_idx];
234 pt.r = color_r[rgb_idx];
240 template <
typename Po
intT>
bool 244 uint32_t uncompressed_size;
245 std::vector<char> compressed_data;
246 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
248 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
254 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 ());
258 std::vector<char> uncompressed_data (uncompressed_size);
259 decompress (compressed_data, uncompressed_data);
261 if (uncompressed_data.empty ())
263 PCL_ERROR (
"[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
272 unsigned char *color_r =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
273 unsigned char *color_g =
reinterpret_cast<unsigned char*
> (&uncompressed_data[
getWidth () *
getHeight ()]);
274 unsigned char *color_b =
reinterpret_cast<unsigned char*
> (&uncompressed_data[2 *
getWidth () *
getHeight ()]);
277 #pragma omp parallel for num_threads (num_threads) 279 for (
long int i = 0; i < cloud.
size (); ++i)
291 template <
typename Po
intT>
bool 295 uint32_t uncompressed_size;
296 std::vector<char> compressed_data;
297 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
299 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
305 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 ());
309 std::vector<char> uncompressed_data (uncompressed_size);
310 decompress (compressed_data, uncompressed_data);
312 if (uncompressed_data.empty ())
314 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
324 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
325 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
326 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
329 for (
int i = 0; i < wh2; ++i, y_idx += 2)
331 int v = color_v[i] - 128;
332 int u = color_u[i] - 128;
335 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
336 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
337 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
340 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
341 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
342 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
349 template <
typename Po
intT>
bool 353 uint32_t uncompressed_size;
354 std::vector<char> compressed_data;
355 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
357 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
363 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 ());
367 std::vector<char> uncompressed_data (uncompressed_size);
368 decompress (compressed_data, uncompressed_data);
370 if (uncompressed_data.empty ())
372 PCL_ERROR (
"[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
382 unsigned char *color_u =
reinterpret_cast<unsigned char*
> (&uncompressed_data[0]);
383 unsigned char *color_y =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2]);
384 unsigned char *color_v =
reinterpret_cast<unsigned char*
> (&uncompressed_data[wh2 +
getWidth () *
getHeight ()]);
387 #pragma omp parallel for num_threads (num_threads) 389 for (
int i = 0; i < wh2; ++i)
392 int v = color_v[i] - 128;
393 int u = color_u[i] - 128;
396 pt1.r = CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
397 pt1.g = CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
398 pt1.b = CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));
401 pt2.r = CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
402 pt2.g = CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
403 pt2.b = CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
410 template <
typename Po
intT>
bool 414 uint32_t uncompressed_size;
415 std::vector<char> compressed_data;
416 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
418 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
424 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 ());
428 std::vector<char> uncompressed_data (uncompressed_size);
429 decompress (compressed_data, uncompressed_data);
431 if (uncompressed_data.empty ())
433 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
440 i.
debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
441 static_cast<unsigned char*> (&rgb_buffer[0]),
448 for (
size_t i = 0; i < cloud.
size (); ++i, rgb_idx += 3)
452 pt.b = rgb_buffer[rgb_idx + 2];
453 pt.g = rgb_buffer[rgb_idx + 1];
454 pt.r = rgb_buffer[rgb_idx + 0];
460 template <
typename Po
intT>
bool 464 uint32_t uncompressed_size;
465 std::vector<char> compressed_data;
466 if (!
loadImageBlob (filename, compressed_data, uncompressed_size))
468 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
474 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 ());
478 std::vector<char> uncompressed_data (uncompressed_size);
479 decompress (compressed_data, uncompressed_data);
481 if (uncompressed_data.empty ())
483 PCL_ERROR (
"[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
490 i.
debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
491 static_cast<unsigned char*> (&rgb_buffer[0]),
498 #pragma omp parallel for num_threads (num_threads) 500 for (
long int i = 0; i < cloud.
size (); ++i)
503 long int rgb_idx = 3*i;
504 pt.b = rgb_buffer[rgb_idx + 2];
505 pt.g = rgb_buffer[rgb_idx + 1];
506 pt.r = rgb_buffer[rgb_idx + 0];
511 #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...
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).
uint32_t getWidth() const
Get the image width as read from disk.
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 is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool loadImageBlob(const std::string &filename, std::vector< char > &data, uint32_t &uncompressed_size)
Load a compressed image array from disk.
std::string getImageType() const
Get the type of the image read 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.
bool decompress(const std::vector< char > &input, std::vector< char > &output)
Realtime LZF decompression.
uint32_t getHeight() const
Get the image height as read from disk.