Point Cloud Library (PCL)  1.14.1-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  const auto cloud_size = cloud_arg.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[i];
112 
113  if (pcl::isFinite (point))
114  {
115  // Inverse depth quantization
116  auto 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.clear ();
153  cloud_arg.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.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.clear ();
218  cloud_arg.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  // Generate new points
243  newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
244  newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
245  newPoint.z = pixel_depth;
246 
247  }
248  else
249  {
250  // Generate bad point
251  newPoint.x = newPoint.y = newPoint.z = bad_point;
252  }
253 
254  cloud_arg.push_back (newPoint);
255  }
256  }
257 };
258 
259 // Colored point cloud specialization
260 template <typename PointT>
262 {
263  /** \brief Convert point cloud to disparity image and rgb image
264  * \param[in] cloud_arg input point cloud
265  * \param[in] focalLength_arg focal length
266  * \param[in] disparityShift_arg disparity shift
267  * \param[in] disparityScale_arg disparity scaling
268  * \param[in] convertToMono convert color to mono/grayscale
269  * \param[out] disparityData_arg output disparity image
270  * \param[out] rgbData_arg output rgb image
271  * \ingroup io
272  */
273  static void convert(const pcl::PointCloud<PointT>& cloud_arg,
274  float focalLength_arg,
275  float disparityShift_arg,
276  float disparityScale_arg,
277  bool convertToMono,
278  typename std::vector<std::uint16_t>& disparityData_arg,
279  typename std::vector<std::uint8_t>& rgbData_arg)
280  {
281  const auto cloud_size = cloud_arg.size ();
282 
283  // Reset output vectors
284  disparityData_arg.clear ();
285  rgbData_arg.clear ();
286 
287  // Allocate memory
288  disparityData_arg.reserve (cloud_size);
289  if (convertToMono)
290  {
291  rgbData_arg.reserve (cloud_size);
292  } else
293  {
294  rgbData_arg.reserve (cloud_size * 3);
295  }
296 
297  for (std::size_t i = 0; i < cloud_size; ++i)
298  {
299  const PointT& point = cloud_arg[i];
300 
301  if (pcl::isFinite (point))
302  {
303  if (convertToMono)
304  {
305  // Encode point color
306  auto grayvalue = static_cast<std::uint8_t>(0.2989 * point.r
307  + 0.5870 * point.g
308  + 0.1140 * point.b);
309 
310  rgbData_arg.push_back (grayvalue);
311  } else
312  {
313  // Encode point color
314  rgbData_arg.push_back (point.r);
315  rgbData_arg.push_back (point.g);
316  rgbData_arg.push_back (point.b);
317  }
318 
319  // Inverse depth quantization
320  auto disparity = static_cast<std::uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
321 
322  // Encode disparity
323  disparityData_arg.push_back (disparity);
324  }
325  else
326  {
327  // Encode black point
328  if (convertToMono)
329  {
330  rgbData_arg.push_back (0);
331  } else
332  {
333  rgbData_arg.push_back (0);
334  rgbData_arg.push_back (0);
335  rgbData_arg.push_back (0);
336  }
337 
338  // Encode bad point
339  disparityData_arg.push_back (0);
340  }
341  }
342  }
343 
344  /** \brief Convert disparity image to point cloud
345  * \param[in] disparityData_arg output disparity image
346  * \param[in] rgbData_arg output rgb image
347  * \param[in] monoImage_arg input image is a single-channel mono image
348  * \param[in] width_arg width of disparity image
349  * \param[in] height_arg height of disparity image
350  * \param[in] focalLength_arg focal length
351  * \param[in] disparityShift_arg disparity shift
352  * \param[in] disparityScale_arg disparity scaling
353  * \param[out] cloud_arg output point cloud
354  * \ingroup io
355  */
356  static void convert(typename std::vector<std::uint16_t>& disparityData_arg,
357  typename std::vector<std::uint8_t>& rgbData_arg,
358  bool monoImage_arg,
359  std::size_t width_arg,
360  std::size_t height_arg,
361  float focalLength_arg,
362  float disparityShift_arg,
363  float disparityScale_arg,
364  pcl::PointCloud<PointT>& cloud_arg)
365  {
366  std::size_t cloud_size = width_arg*height_arg;
367  bool hasColor = (!rgbData_arg.empty ());
368 
369  // Check size of input data
370  assert (disparityData_arg.size()==cloud_size);
371  if (hasColor)
372  {
373  if (monoImage_arg)
374  {
375  assert (rgbData_arg.size()==cloud_size);
376  } else
377  {
378  assert (rgbData_arg.size()==cloud_size*3);
379  }
380  }
381 
382  // Reset point cloud
383  cloud_arg.clear();
384  cloud_arg.reserve(cloud_size);
385 
386  // Define point cloud parameters
387  cloud_arg.width = static_cast<std::uint32_t>(width_arg);
388  cloud_arg.height = static_cast<std::uint32_t>(height_arg);
389  cloud_arg.is_dense = false;
390 
391  // Calculate center of disparity image
392  int centerX = static_cast<int>(width_arg/2);
393  int centerY = static_cast<int>(height_arg/2);
394 
395  const float fl_const = 1.0f/focalLength_arg;
396  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
397 
398  std::size_t i = 0;
399  for (int y = -centerY; y < centerY; ++y )
400  for (int x = -centerX; x < centerX; ++x )
401  {
402  PointT newPoint;
403 
404  const std::uint16_t& pixel_disparity = disparityData_arg[i];
405 
406  if (pixel_disparity && (pixel_disparity!=0x7FF))
407  {
408  float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
409 
410  // Define point location
411  newPoint.z = depth;
412  newPoint.x = static_cast<float> (x) * depth * fl_const;
413  newPoint.y = static_cast<float> (y) * depth * fl_const;
414 
415  if (hasColor)
416  {
417  if (monoImage_arg)
418  {
419  // Define point color
420  newPoint.r = rgbData_arg[i];
421  newPoint.g = rgbData_arg[i];
422  newPoint.b = rgbData_arg[i];
423  } else
424  {
425  // Define point color
426  newPoint.r = rgbData_arg[i*3+0];
427  newPoint.g = rgbData_arg[i*3+1];
428  newPoint.b = rgbData_arg[i*3+2];
429  }
430 
431  } else
432  {
433  // Set white point color
434  newPoint.rgba = 0xffffffffu;
435  }
436  } else
437  {
438  // Define bad point
439  newPoint.x = newPoint.y = newPoint.z = bad_point;
440  newPoint.rgb = 0.0f;
441  }
442 
443  // Add point to cloud
444  cloud_arg.push_back(newPoint);
445  // Increment point iterator
446  ++i;
447  }
448  }
449 
450  /** \brief Convert disparity image to point cloud
451  * \param[in] depthData_arg output disparity image
452  * \param[in] rgbData_arg output rgb image
453  * \param[in] monoImage_arg input image is a single-channel mono image
454  * \param[in] width_arg width of disparity image
455  * \param[in] height_arg height of disparity image
456  * \param[in] focalLength_arg focal length
457  * \param[out] cloud_arg output point cloud
458  * \ingroup io
459  */
460  static void convert(typename std::vector<float>& depthData_arg,
461  typename std::vector<std::uint8_t>& rgbData_arg,
462  bool monoImage_arg,
463  std::size_t width_arg,
464  std::size_t height_arg,
465  float focalLength_arg,
466  pcl::PointCloud<PointT>& cloud_arg)
467  {
468  std::size_t cloud_size = width_arg*height_arg;
469  bool hasColor = (!rgbData_arg.empty ());
470 
471  // Check size of input data
472  assert (depthData_arg.size()==cloud_size);
473  if (hasColor)
474  {
475  if (monoImage_arg)
476  {
477  assert (rgbData_arg.size()==cloud_size);
478  } else
479  {
480  assert (rgbData_arg.size()==cloud_size*3);
481  }
482  }
483 
484  // Reset point cloud
485  cloud_arg.clear();
486  cloud_arg.reserve(cloud_size);
487 
488  // Define point cloud parameters
489  cloud_arg.width = static_cast<std::uint32_t>(width_arg);
490  cloud_arg.height = static_cast<std::uint32_t>(height_arg);
491  cloud_arg.is_dense = false;
492 
493  // Calculate center of disparity image
494  int centerX = static_cast<int>(width_arg/2);
495  int centerY = static_cast<int>(height_arg/2);
496 
497  const float fl_const = 1.0f/focalLength_arg;
498  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
499 
500  std::size_t i = 0;
501  for (int y = -centerY; y < centerY; ++y )
502  for (int x = -centerX; x < centerX; ++x )
503  {
504  PointT newPoint;
505 
506  const float& pixel_depth = depthData_arg[i];
507 
508  if (pixel_depth)
509  {
510  // Define point location
511  newPoint.z = pixel_depth;
512  newPoint.x = static_cast<float> (x) * pixel_depth * fl_const;
513  newPoint.y = static_cast<float> (y) * pixel_depth * fl_const;
514 
515  if (hasColor)
516  {
517  if (monoImage_arg)
518  {
519  // Define point color
520  newPoint.r = rgbData_arg[i];
521  newPoint.g = rgbData_arg[i];
522  newPoint.b = rgbData_arg[i];
523  } else
524  {
525  // Define point color
526  newPoint.r = rgbData_arg[i*3+0];
527  newPoint.g = rgbData_arg[i*3+1];
528  newPoint.b = rgbData_arg[i*3+2];
529  }
530 
531  } else
532  {
533  // Set white point color
534  newPoint.rgba = 0xffffffffu;
535  }
536  } else
537  {
538  // Define bad point
539  newPoint.x = newPoint.y = newPoint.z = bad_point;
540  newPoint.rgb = 0.0f;
541  }
542 
543  // Add point to cloud
544  cloud_arg.push_back(newPoint);
545  // Increment point iterator
546  ++i;
547  }
548  }
549 };
550 
551 } // namespace io
552 } // namespace pcl
553 
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
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:403
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
void reserve(std::size_t n)
Definition: point_cloud.h:445
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.
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.
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.
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.
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.
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.
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
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.