Point Cloud Library (PCL)  1.8.1-dev
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
organized_pointcloud_conversion.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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  * $Id$
37  * Authors: Julius Kammerl (julius@kammerl.de)
38  */
39 
40 #ifndef PCL_ORGANIZED_CONVERSION_H_
41 #define PCL_ORGANIZED_CONVERSION_H_
42 
43 #include <pcl/pcl_macros.h>
44 #include <pcl/point_cloud.h>
45 
46 #include <vector>
47 #include <limits>
48 #include <assert.h>
49 
50 namespace pcl
51 {
52  namespace io
53  {
54 
55  template<typename PointT> struct CompressionPointTraits
56  {
57  static const bool hasColor = false;
58  static const unsigned int channels = 1;
59  static const size_t bytesPerPoint = 3 * sizeof(float);
60  };
61 
62  template<>
64  {
65  static const bool hasColor = true;
66  static const unsigned int channels = 4;
67  static const size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(uint8_t);
68  };
69 
70  template<>
72  {
73  static const bool hasColor = true;
74  static const unsigned int channels = 4;
75  static const size_t bytesPerPoint = 3 * sizeof(float) + 3 * sizeof(uint8_t);
76  };
77 
78  //////////////////////////////////////////////////////////////////////////////////////////////
79  template <typename PointT, bool enableColor = CompressionPointTraits<PointT>::hasColor >
81 
82  //////////////////////////////////////////////////////////////////////////////////////////////
83  // Uncolored point cloud specialization
84  //////////////////////////////////////////////////////////////////////////////////////////////
85  template<typename PointT>
86  struct OrganizedConversion<PointT, false>
87  {
88  /** \brief Convert point cloud to disparity image
89  * \param[in] cloud_arg input point cloud
90  * \param[in] focalLength_arg focal length
91  * \param[in] disparityShift_arg disparity shift
92  * \param[in] disparityScale_arg disparity scaling
93  * \param[out] disparityData_arg output disparity image
94  * \ingroup io
95  */
96  static void convert(const pcl::PointCloud<PointT>& cloud_arg,
97  float focalLength_arg,
98  float disparityShift_arg,
99  float disparityScale_arg,
100  bool ,
101  typename std::vector<uint16_t>& disparityData_arg,
102  typename std::vector<uint8_t>&)
103  {
104  size_t cloud_size, i;
105 
106  cloud_size = cloud_arg.points.size ();
107 
108  // Clear image data
109  disparityData_arg.clear ();
110 
111  disparityData_arg.reserve (cloud_size);
112 
113  for (i = 0; i < cloud_size; ++i)
114  {
115  // Get point from cloud
116  const PointT& point = cloud_arg.points[i];
117 
118  if (pcl::isFinite (point))
119  {
120  // Inverse depth quantization
121  uint16_t disparity = static_cast<uint16_t> ( focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
122  disparityData_arg.push_back (disparity);
123  }
124  else
125  {
126  // Non-valid points are encoded with zeros
127  disparityData_arg.push_back (0);
128  }
129  }
130  }
131 
132  /** \brief Convert disparity image to point cloud
133  * \param[in] disparityData_arg input depth image
134  * \param[in] width_arg width of disparity image
135  * \param[in] height_arg height of disparity image
136  * \param[in] focalLength_arg focal length
137  * \param[in] disparityShift_arg disparity shift
138  * \param[in] disparityScale_arg disparity scaling
139  * \param[out] cloud_arg output point cloud
140  * \ingroup io
141  */
142  static void convert(typename std::vector<uint16_t>& disparityData_arg,
143  typename std::vector<uint8_t>&,
144  bool,
145  size_t width_arg,
146  size_t height_arg,
147  float focalLength_arg,
148  float disparityShift_arg,
149  float disparityScale_arg,
150  pcl::PointCloud<PointT>& cloud_arg)
151  {
152  size_t i;
153  size_t cloud_size = width_arg * height_arg;
154  int x, y, centerX, centerY;
155 
156  assert(disparityData_arg.size()==cloud_size);
157 
158  // Reset point cloud
159  cloud_arg.points.clear ();
160  cloud_arg.points.reserve (cloud_size);
161 
162  // Define point cloud parameters
163  cloud_arg.width = static_cast<uint32_t> (width_arg);
164  cloud_arg.height = static_cast<uint32_t> (height_arg);
165  cloud_arg.is_dense = false;
166 
167  // Calculate center of disparity image
168  centerX = static_cast<int> (width_arg / 2);
169  centerY = static_cast<int> (height_arg / 2);
170 
171  const float fl_const = 1.0f / focalLength_arg;
172  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
173 
174  i = 0;
175  for (y = -centerY; y < +centerY; ++y)
176  for (x = -centerX; x < +centerX; ++x)
177  {
178  PointT newPoint;
179  const uint16_t& pixel_disparity = disparityData_arg[i];
180  ++i;
181 
182  if (pixel_disparity)
183  {
184  // Inverse depth decoding
185  float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
186 
187  // Generate new points
188  newPoint.x = static_cast<float> (x) * depth * fl_const;
189  newPoint.y = static_cast<float> (y) * depth * fl_const;
190  newPoint.z = depth;
191 
192  }
193  else
194  {
195  // Generate bad point
196  newPoint.x = newPoint.y = newPoint.z = bad_point;
197  }
198 
199  cloud_arg.points.push_back (newPoint);
200  }
201 
202  }
203 
204 
205  /** \brief Convert disparity image to point cloud
206  * \param[in] depthData_arg input depth image
207  * \param[in] width_arg width of disparity image
208  * \param[in] height_arg height of disparity image
209  * \param[in] focalLength_arg focal length
210  * \param[out] cloud_arg output point cloud
211  * \ingroup io
212  */
213  static void convert(typename std::vector<float>& depthData_arg,
214  typename std::vector<uint8_t>&,
215  bool,
216  size_t width_arg,
217  size_t height_arg,
218  float focalLength_arg,
219  pcl::PointCloud<PointT>& cloud_arg)
220  {
221  size_t i;
222  size_t cloud_size = width_arg * height_arg;
223  int x, y, centerX, centerY;
224 
225  assert(depthData_arg.size()==cloud_size);
226 
227  // Reset point cloud
228  cloud_arg.points.clear ();
229  cloud_arg.points.reserve (cloud_size);
230 
231  // Define point cloud parameters
232  cloud_arg.width = static_cast<uint32_t> (width_arg);
233  cloud_arg.height = static_cast<uint32_t> (height_arg);
234  cloud_arg.is_dense = false;
235 
236  // Calculate center of disparity image
237  centerX = static_cast<int> (width_arg / 2);
238  centerY = static_cast<int> (height_arg / 2);
239 
240  const float fl_const = 1.0f / focalLength_arg;
241  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
242 
243  i = 0;
244  for (y = -centerY; y < +centerY; ++y)
245  for (x = -centerX; x < +centerX; ++x)
246  {
247  PointT newPoint;
248  const float& pixel_depth = depthData_arg[i];
249  ++i;
250 
251  if (pixel_depth)
252  {
253  // Inverse depth decoding
254  float depth = focalLength_arg / pixel_depth;
255 
256  // Generate new points
257  newPoint.x = static_cast<float> (x) * depth * fl_const;
258  newPoint.y = static_cast<float> (y) * depth * fl_const;
259  newPoint.z = depth;
260 
261  }
262  else
263  {
264  // Generate bad point
265  newPoint.x = newPoint.y = newPoint.z = bad_point;
266  }
267 
268  cloud_arg.points.push_back (newPoint);
269  }
270 
271  }
272 
273  };
274 
275  //////////////////////////////////////////////////////////////////////////////////////////////
276  // Colored point cloud specialization
277  //////////////////////////////////////////////////////////////////////////////////////////////
278  template <typename PointT>
280  {
281  /** \brief Convert point cloud to disparity image and rgb image
282  * \param[in] cloud_arg input point cloud
283  * \param[in] focalLength_arg focal length
284  * \param[in] disparityShift_arg disparity shift
285  * \param[in] disparityScale_arg disparity scaling
286  * \param[in] convertToMono convert color to mono/grayscale
287  * \param[out] disparityData_arg output disparity image
288  * \param[out] rgbData_arg output rgb image
289  * \ingroup io
290  */
291  static void convert(const pcl::PointCloud<PointT>& cloud_arg,
292  float focalLength_arg,
293  float disparityShift_arg,
294  float disparityScale_arg,
295  bool convertToMono,
296  typename std::vector<uint16_t>& disparityData_arg,
297  typename std::vector<uint8_t>& rgbData_arg)
298  {
299  size_t cloud_size, i;
300 
301  cloud_size = cloud_arg.points.size ();
302 
303  // Reset output vectors
304  disparityData_arg.clear ();
305  rgbData_arg.clear ();
306 
307  // Allocate memory
308  disparityData_arg.reserve (cloud_size);
309  if (convertToMono)
310  {
311  rgbData_arg.reserve (cloud_size);
312  } else
313  {
314  rgbData_arg.reserve (cloud_size * 3);
315  }
316 
317 
318  for (i = 0; i < cloud_size; ++i)
319  {
320  const PointT& point = cloud_arg.points[i];
321 
322  if (pcl::isFinite (point))
323  {
324  if (convertToMono)
325  {
326  // Encode point color
327  uint8_t grayvalue = static_cast<uint8_t>(0.2989 * point.r
328  + 0.5870 * point.g
329  + 0.1140 * point.b);
330 
331  rgbData_arg.push_back (grayvalue);
332  } else
333  {
334  // Encode point color
335  rgbData_arg.push_back (point.r);
336  rgbData_arg.push_back (point.g);
337  rgbData_arg.push_back (point.b);
338  }
339 
340 
341  // Inverse depth quantization
342  uint16_t disparity = static_cast<uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
343 
344  // Encode disparity
345  disparityData_arg.push_back (disparity);
346  }
347  else
348  {
349 
350  // Encode black point
351  if (convertToMono)
352  {
353  rgbData_arg.push_back (0);
354  } else
355  {
356  rgbData_arg.push_back (0);
357  rgbData_arg.push_back (0);
358  rgbData_arg.push_back (0);
359  }
360 
361  // Encode bad point
362  disparityData_arg.push_back (0);
363  }
364  }
365 
366  }
367 
368  /** \brief Convert disparity image to point cloud
369  * \param[in] disparityData_arg output disparity image
370  * \param[in] rgbData_arg output rgb image
371  * \param[in] monoImage_arg input image is a single-channel mono image
372  * \param[in] width_arg width of disparity image
373  * \param[in] height_arg height of disparity image
374  * \param[in] focalLength_arg focal length
375  * \param[in] disparityShift_arg disparity shift
376  * \param[in] disparityScale_arg disparity scaling
377  * \param[out] cloud_arg output point cloud
378  * \ingroup io
379  */
380  static void convert(typename std::vector<uint16_t>& disparityData_arg,
381  typename std::vector<uint8_t>& rgbData_arg,
382  bool monoImage_arg,
383  size_t width_arg,
384  size_t height_arg,
385  float focalLength_arg,
386  float disparityShift_arg,
387  float disparityScale_arg,
388  pcl::PointCloud<PointT>& cloud_arg)
389  {
390  size_t i;
391  size_t cloud_size = width_arg*height_arg;
392  bool hasColor = (rgbData_arg.size () > 0);
393 
394  // Check size of input data
395  assert (disparityData_arg.size()==cloud_size);
396  if (hasColor)
397  {
398  if (monoImage_arg)
399  {
400  assert (rgbData_arg.size()==cloud_size);
401  } else
402  {
403  assert (rgbData_arg.size()==cloud_size*3);
404  }
405  }
406 
407  int x, y, centerX, centerY;
408 
409  // Reset point cloud
410  cloud_arg.points.clear();
411  cloud_arg.points.reserve(cloud_size);
412 
413  // Define point cloud parameters
414  cloud_arg.width = static_cast<uint32_t>(width_arg);
415  cloud_arg.height = static_cast<uint32_t>(height_arg);
416  cloud_arg.is_dense = false;
417 
418  // Calculate center of disparity image
419  centerX = static_cast<int>(width_arg/2);
420  centerY = static_cast<int>(height_arg/2);
421 
422  const float fl_const = 1.0f/focalLength_arg;
423  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
424 
425  i = 0;
426  for (y=-centerY; y<+centerY; ++y )
427  for (x=-centerX; x<+centerX; ++x )
428  {
429  PointT newPoint;
430 
431  const uint16_t& pixel_disparity = disparityData_arg[i];
432 
433  if (pixel_disparity && (pixel_disparity!=0x7FF))
434  {
435  float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
436 
437  // Define point location
438  newPoint.z = depth;
439  newPoint.x = static_cast<float> (x) * depth * fl_const;
440  newPoint.y = static_cast<float> (y) * depth * fl_const;
441 
442  if (hasColor)
443  {
444  if (monoImage_arg)
445  {
446  // Define point color
447  newPoint.r = rgbData_arg[i];
448  newPoint.g = rgbData_arg[i];
449  newPoint.b = rgbData_arg[i];
450  } else
451  {
452  // Define point color
453  newPoint.r = rgbData_arg[i*3+0];
454  newPoint.g = rgbData_arg[i*3+1];
455  newPoint.b = rgbData_arg[i*3+2];
456  }
457 
458  } else
459  {
460  // Set white point color
461  newPoint.rgba = 0xffffffffu;
462  }
463  } else
464  {
465  // Define bad point
466  newPoint.x = newPoint.y = newPoint.z = bad_point;
467  newPoint.rgb = 0.0f;
468  }
469 
470  // Add point to cloud
471  cloud_arg.points.push_back(newPoint);
472  // Increment point iterator
473  ++i;
474  }
475  }
476 
477  /** \brief Convert disparity image to point cloud
478  * \param[in] depthData_arg output disparity image
479  * \param[in] rgbData_arg output rgb image
480  * \param[in] monoImage_arg input image is a single-channel mono image
481  * \param[in] width_arg width of disparity image
482  * \param[in] height_arg height of disparity image
483  * \param[in] focalLength_arg focal length
484  * \param[out] cloud_arg output point cloud
485  * \ingroup io
486  */
487  static void convert(typename std::vector<float>& depthData_arg,
488  typename std::vector<uint8_t>& rgbData_arg,
489  bool monoImage_arg,
490  size_t width_arg,
491  size_t height_arg,
492  float focalLength_arg,
493  pcl::PointCloud<PointT>& cloud_arg)
494  {
495  size_t i;
496  size_t cloud_size = width_arg*height_arg;
497  bool hasColor = (rgbData_arg.size () > 0);
498 
499  // Check size of input data
500  assert (depthData_arg.size()==cloud_size);
501  if (hasColor)
502  {
503  if (monoImage_arg)
504  {
505  assert (rgbData_arg.size()==cloud_size);
506  } else
507  {
508  assert (rgbData_arg.size()==cloud_size*3);
509  }
510  }
511 
512  int x, y, centerX, centerY;
513 
514  // Reset point cloud
515  cloud_arg.points.clear();
516  cloud_arg.points.reserve(cloud_size);
517 
518  // Define point cloud parameters
519  cloud_arg.width = static_cast<uint32_t>(width_arg);
520  cloud_arg.height = static_cast<uint32_t>(height_arg);
521  cloud_arg.is_dense = false;
522 
523  // Calculate center of disparity image
524  centerX = static_cast<int>(width_arg/2);
525  centerY = static_cast<int>(height_arg/2);
526 
527  const float fl_const = 1.0f/focalLength_arg;
528  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
529 
530  i = 0;
531  for (y=-centerY; y<+centerY; ++y )
532  for (x=-centerX; x<+centerX; ++x )
533  {
534  PointT newPoint;
535 
536  const float& pixel_depth = depthData_arg[i];
537 
538  if (pixel_depth==pixel_depth)
539  {
540  float depth = focalLength_arg / pixel_depth;
541 
542  // Define point location
543  newPoint.z = depth;
544  newPoint.x = static_cast<float> (x) * depth * fl_const;
545  newPoint.y = static_cast<float> (y) * depth * fl_const;
546 
547  if (hasColor)
548  {
549  if (monoImage_arg)
550  {
551  // Define point color
552  newPoint.r = rgbData_arg[i];
553  newPoint.g = rgbData_arg[i];
554  newPoint.b = rgbData_arg[i];
555  } else
556  {
557  // Define point color
558  newPoint.r = rgbData_arg[i*3+0];
559  newPoint.g = rgbData_arg[i*3+1];
560  newPoint.b = rgbData_arg[i*3+2];
561  }
562 
563  } else
564  {
565  // Set white point color
566  newPoint.rgba = 0xffffffffu;
567  }
568  } else
569  {
570  // Define bad point
571  newPoint.x = newPoint.y = newPoint.z = bad_point;
572  newPoint.rgb = 0.0f;
573  }
574 
575  // Add point to cloud
576  cloud_arg.points.push_back(newPoint);
577  // Increment point iterator
578  ++i;
579  }
580  }
581  };
582 
583  }
584 }
585 
586 
587 #endif
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool, typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &)
Convert point cloud to disparity image.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:54
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
static void convert(typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &, bool, size_t width_arg, size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< uint8_t > &, bool, size_t width_arg, size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
static void convert(const pcl::PointCloud< PointT > &cloud_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, bool convertToMono, typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &rgbData_arg)
Convert point cloud to disparity image and rgb image.
static void convert(typename std::vector< uint16_t > &disparityData_arg, typename std::vector< uint8_t > &rgbData_arg, bool monoImage_arg, size_t width_arg, size_t height_arg, float focalLength_arg, float disparityShift_arg, float disparityScale_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
static void convert(typename std::vector< float > &depthData_arg, typename std::vector< uint8_t > &rgbData_arg, bool monoImage_arg, size_t width_arg, size_t height_arg, float focalLength_arg, pcl::PointCloud< PointT > &cloud_arg)
Convert disparity image to point cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415