Point Cloud Library (PCL)  1.7.1
lzf_image_io.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_LZF_IMAGE_IO_HPP_
39 #define PCL_LZF_IMAGE_IO_HPP_
40 
41 #include <pcl/console/print.h>
42 #include <pcl/io/debayer.h>
43 
44 #define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))
45 
46 //////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> bool
49  const std::string &filename, pcl::PointCloud<PointT> &cloud)
50 {
51  uint32_t uncompressed_size;
52  std::vector<char> compressed_data;
53  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
54  {
55  PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
56  return (false);
57  }
58 
59  if (uncompressed_size != getWidth () * getHeight () * 2)
60  {
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 ());
62  return (false);
63  }
64 
65  std::vector<char> uncompressed_data (uncompressed_size);
66  decompress (compressed_data, uncompressed_data);
67 
68  if (uncompressed_data.empty ())
69  {
70  PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
71  return (false);
72  }
73 
74  // Copy to PointT
75  cloud.width = getWidth ();
76  cloud.height = getHeight ();
77  cloud.is_dense = true;
78  cloud.resize (getWidth () * getHeight ());
79  register int depth_idx = 0, point_idx = 0;
80  double constant_x = 1.0 / parameters_.focal_length_x,
81  constant_y = 1.0 / parameters_.focal_length_y;
82  for (int v = 0; v < cloud.height; ++v)
83  {
84  for (register int u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2)
85  {
86  PointT &pt = cloud.points[point_idx];
87  unsigned short val;
88  memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
89  if (val == 0)
90  {
91  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
92  cloud.is_dense = false;
93  continue;
94  }
95 
96  pt.z = static_cast<float> (val * z_multiplication_factor_);
97  pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
98  * pt.z * static_cast<float> (constant_x);
99  pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
100  * pt.z * static_cast<float> (constant_y);
101  }
102  }
103  cloud.sensor_origin_.setZero ();
104  cloud.sensor_orientation_.w () = 1.0f;
105  cloud.sensor_orientation_.x () = 0.0f;
106  cloud.sensor_orientation_.y () = 0.0f;
107  cloud.sensor_orientation_.z () = 0.0f;
108  return (true);
109 }
110 
111 ///////////////////////////////////////////////////////////////////////////////
112 template <typename PointT> bool
113 pcl::io::LZFDepth16ImageReader::readOMP (const std::string &filename,
114  pcl::PointCloud<PointT> &cloud,
115  unsigned int num_threads)
116 {
117  uint32_t uncompressed_size;
118  std::vector<char> compressed_data;
119  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
120  {
121  PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
122  return (false);
123  }
124 
125  if (uncompressed_size != getWidth () * getHeight () * 2)
126  {
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 ());
128  return (false);
129  }
130 
131  std::vector<char> uncompressed_data (uncompressed_size);
132  decompress (compressed_data, uncompressed_data);
133 
134  if (uncompressed_data.empty ())
135  {
136  PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
137  return (false);
138  }
139 
140  // Copy to PointT
141  cloud.width = getWidth ();
142  cloud.height = getHeight ();
143  cloud.is_dense = true;
144  cloud.resize (getWidth () * getHeight ());
145  double constant_x = 1.0 / parameters_.focal_length_x,
146  constant_y = 1.0 / parameters_.focal_length_y;
147 #ifdef _OPENMP
148 #pragma omp parallel for num_threads (num_threads)
149 #endif
150  for (int i = 0; i < static_cast< int> (cloud.size ()); ++i)
151  {
152  int u = i % cloud.width;
153  int v = i / cloud.width;
154  PointT &pt = cloud.points[i];
155  int depth_idx = 2*i;
156  unsigned short val;
157  memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
158  if (val == 0)
159  {
160  pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
161  if (cloud.is_dense)
162  {
163 #ifdef _OPENMP
164 #pragma omp critical
165 #endif
166  {
167  if (cloud.is_dense)
168  cloud.is_dense = false;
169  }
170  }
171  continue;
172  }
173 
174  pt.z = static_cast<float> (val * z_multiplication_factor_);
175  pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
176  * pt.z * static_cast<float> (constant_x);
177  pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
178  * pt.z * static_cast<float> (constant_y);
179 
180  }
181  cloud.sensor_origin_.setZero ();
182  cloud.sensor_orientation_.w () = 1.0f;
183  cloud.sensor_orientation_.x () = 0.0f;
184  cloud.sensor_orientation_.y () = 0.0f;
185  cloud.sensor_orientation_.z () = 0.0f;
186  return (true);
187 
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////
191 template <typename PointT> bool
193  const std::string &filename, pcl::PointCloud<PointT> &cloud)
194 {
195  uint32_t uncompressed_size;
196  std::vector<char> compressed_data;
197  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
198  {
199  PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
200  return (false);
201  }
202 
203  if (uncompressed_size != getWidth () * getHeight () * 3)
204  {
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 ());
206  return (false);
207  }
208 
209  std::vector<char> uncompressed_data (uncompressed_size);
210  decompress (compressed_data, uncompressed_data);
211 
212  if (uncompressed_data.empty ())
213  {
214  PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
215  return (false);
216  }
217 
218  // Copy to PointT
219  cloud.width = getWidth ();
220  cloud.height = getHeight ();
221  cloud.resize (getWidth () * getHeight ());
222 
223  register int rgb_idx = 0;
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 ()]);
227 
228  for (size_t i = 0; i < cloud.size (); ++i, ++rgb_idx)
229  {
230  PointT &pt = cloud.points[i];
231 
232  pt.b = color_b[rgb_idx];
233  pt.g = color_g[rgb_idx];
234  pt.r = color_r[rgb_idx];
235  }
236  return (true);
237 }
238 
239 //////////////////////////////////////////////////////////////////////////////
240 template <typename PointT> bool
242  const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
243 {
244  uint32_t uncompressed_size;
245  std::vector<char> compressed_data;
246  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
247  {
248  PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
249  return (false);
250  }
251 
252  if (uncompressed_size != getWidth () * getHeight () * 3)
253  {
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 ());
255  return (false);
256  }
257 
258  std::vector<char> uncompressed_data (uncompressed_size);
259  decompress (compressed_data, uncompressed_data);
260 
261  if (uncompressed_data.empty ())
262  {
263  PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
264  return (false);
265  }
266 
267  // Copy to PointT
268  cloud.width = getWidth ();
269  cloud.height = getHeight ();
270  cloud.resize (getWidth () * getHeight ());
271 
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 ()]);
275 
276 #ifdef _OPENMP
277 #pragma omp parallel for num_threads (num_threads)
278 #endif//_OPENMP
279  for (long int i = 0; i < cloud.size (); ++i)
280  {
281  PointT &pt = cloud.points[i];
282 
283  pt.b = color_b[i];
284  pt.g = color_g[i];
285  pt.r = color_r[i];
286  }
287  return (true);
288 }
289 
290 //////////////////////////////////////////////////////////////////////////////
291 template <typename PointT> bool
293  const std::string &filename, pcl::PointCloud<PointT> &cloud)
294 {
295  uint32_t uncompressed_size;
296  std::vector<char> compressed_data;
297  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
298  {
299  PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
300  return (false);
301  }
302 
303  if (uncompressed_size != getWidth () * getHeight () * 2)
304  {
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 ());
306  return (false);
307  }
308 
309  std::vector<char> uncompressed_data (uncompressed_size);
310  decompress (compressed_data, uncompressed_data);
311 
312  if (uncompressed_data.empty ())
313  {
314  PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
315  return (false);
316  }
317 
318  // Convert YUV422 to RGB24 and copy to PointT
319  cloud.width = getWidth ();
320  cloud.height = getHeight ();
321  cloud.resize (getWidth () * getHeight ());
322 
323  int wh2 = getWidth () * getHeight () / 2;
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 ()]);
327 
328  register int y_idx = 0;
329  for (size_t i = 0; i < wh2; ++i, y_idx += 2)
330  {
331  int v = color_v[i] - 128;
332  int u = color_u[i] - 128;
333 
334  PointT &pt1 = cloud.points[y_idx + 0];
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));
338 
339  PointT &pt2 = cloud.points[y_idx + 1];
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));
343  }
344 
345  return (true);
346 }
347 
348 //////////////////////////////////////////////////////////////////////////////
349 template <typename PointT> bool
351  const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
352 {
353  uint32_t uncompressed_size;
354  std::vector<char> compressed_data;
355  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
356  {
357  PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
358  return (false);
359  }
360 
361  if (uncompressed_size != getWidth () * getHeight () * 2)
362  {
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 ());
364  return (false);
365  }
366 
367  std::vector<char> uncompressed_data (uncompressed_size);
368  decompress (compressed_data, uncompressed_data);
369 
370  if (uncompressed_data.empty ())
371  {
372  PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
373  return (false);
374  }
375 
376  // Convert YUV422 to RGB24 and copy to PointT
377  cloud.width = getWidth ();
378  cloud.height = getHeight ();
379  cloud.resize (getWidth () * getHeight ());
380 
381  int wh2 = getWidth () * getHeight () / 2;
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 ()]);
385 
386 #ifdef _OPENMP
387 #pragma omp parallel for num_threads (num_threads)
388 #endif//_OPENMP
389  for (int i = 0; i < wh2; ++i)
390  {
391  int y_idx = 2*i;
392  int v = color_v[i] - 128;
393  int u = color_u[i] - 128;
394 
395  PointT &pt1 = cloud.points[y_idx + 0];
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));
399 
400  PointT &pt2 = cloud.points[y_idx + 1];
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));
404  }
405 
406  return (true);
407 }
408 
409 //////////////////////////////////////////////////////////////////////////////
410 template <typename PointT> bool
412  const std::string &filename, pcl::PointCloud<PointT> &cloud)
413 {
414  uint32_t uncompressed_size;
415  std::vector<char> compressed_data;
416  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
417  {
418  PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
419  return (false);
420  }
421 
422  if (uncompressed_size != getWidth () * getHeight ())
423  {
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 ());
425  return (false);
426  }
427 
428  std::vector<char> uncompressed_data (uncompressed_size);
429  decompress (compressed_data, uncompressed_data);
430 
431  if (uncompressed_data.empty ())
432  {
433  PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
434  return (false);
435  }
436 
437  // Convert Bayer8 to RGB24
438  std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
440  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
441  static_cast<unsigned char*> (&rgb_buffer[0]),
442  getWidth (), getHeight ());
443  // Copy to PointT
444  cloud.width = getWidth ();
445  cloud.height = getHeight ();
446  cloud.resize (getWidth () * getHeight ());
447  register int rgb_idx = 0;
448  for (size_t i = 0; i < cloud.size (); ++i, rgb_idx += 3)
449  {
450  PointT &pt = cloud.points[i];
451 
452  pt.b = rgb_buffer[rgb_idx + 2];
453  pt.g = rgb_buffer[rgb_idx + 1];
454  pt.r = rgb_buffer[rgb_idx + 0];
455  }
456  return (true);
457 }
458 
459 //////////////////////////////////////////////////////////////////////////////
460 template <typename PointT> bool
462  const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
463 {
464  uint32_t uncompressed_size;
465  std::vector<char> compressed_data;
466  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
467  {
468  PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
469  return (false);
470  }
471 
472  if (uncompressed_size != getWidth () * getHeight ())
473  {
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 ());
475  return (false);
476  }
477 
478  std::vector<char> uncompressed_data (uncompressed_size);
479  decompress (compressed_data, uncompressed_data);
480 
481  if (uncompressed_data.empty ())
482  {
483  PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
484  return (false);
485  }
486 
487  // Convert Bayer8 to RGB24
488  std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
490  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
491  static_cast<unsigned char*> (&rgb_buffer[0]),
492  getWidth (), getHeight ());
493  // Copy to PointT
494  cloud.width = getWidth ();
495  cloud.height = getHeight ();
496  cloud.resize (getWidth () * getHeight ());
497 #ifdef _OPENMP
498 #pragma omp parallel for num_threads (num_threads)
499 #endif//_OPENMP
500  for (long int i = 0; i < cloud.size (); ++i)
501  {
502  PointT &pt = cloud.points[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];
507  }
508  return (true);
509 }
510 
511 #endif //#ifndef PCL_LZF_IMAGE_IO_HPP_
512