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