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