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