Point Cloud Library (PCL)  1.7.0
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  const uint32_t rgb = *reinterpret_cast<const int*> (&point.rgb);
328  uint8_t grayvalue = static_cast<uint8_t>(0.2989 * static_cast<float>((rgb >> 16) & 0x0000ff) +
329  0.5870 * static_cast<float>((rgb >> 8) & 0x0000ff) +
330  0.1140 * static_cast<float>((rgb >> 0) & 0x0000ff));
331 
332  rgbData_arg.push_back (grayvalue);
333  } else
334  {
335  // Encode point color
336  const uint32_t rgb = *reinterpret_cast<const int*> (&point.rgb);
337 
338  rgbData_arg.push_back ( (rgb >> 16) & 0x0000ff);
339  rgbData_arg.push_back ( (rgb >> 8) & 0x0000ff);
340  rgbData_arg.push_back ( (rgb >> 0) & 0x0000ff);
341  }
342 
343 
344  // Inverse depth quantization
345  uint16_t disparity = static_cast<uint16_t> (focalLength_arg / (disparityScale_arg * point.z) + disparityShift_arg / disparityScale_arg);
346 
347  // Encode disparity
348  disparityData_arg.push_back (disparity);
349  }
350  else
351  {
352 
353  // Encode black point
354  if (convertToMono)
355  {
356  rgbData_arg.push_back (0);
357  } else
358  {
359  rgbData_arg.push_back (0);
360  rgbData_arg.push_back (0);
361  rgbData_arg.push_back (0);
362  }
363 
364  // Encode bad point
365  disparityData_arg.push_back (0);
366  }
367  }
368 
369  }
370 
371  /** \brief Convert disparity image to point cloud
372  * \param[in] disparityData_arg output disparity image
373  * \param[in] rgbData_arg output rgb image
374  * \param[in] monoImage_arg input image is a single-channel mono image
375  * \param[in] width_arg width of disparity image
376  * \param[in] height_arg height of disparity image
377  * \param[in] focalLength_arg focal length
378  * \param[in] disparityShift_arg disparity shift
379  * \param[in] disparityScale_arg disparity scaling
380  * \param[out] cloud_arg output point cloud
381  * \ingroup io
382  */
383  static void convert(typename std::vector<uint16_t>& disparityData_arg,
384  typename std::vector<uint8_t>& rgbData_arg,
385  bool monoImage_arg,
386  size_t width_arg,
387  size_t height_arg,
388  float focalLength_arg,
389  float disparityShift_arg,
390  float disparityScale_arg,
391  pcl::PointCloud<PointT>& cloud_arg)
392  {
393  size_t i;
394  size_t cloud_size = width_arg*height_arg;
395  bool hasColor = (rgbData_arg.size () > 0);
396 
397  // Check size of input data
398  assert (disparityData_arg.size()==cloud_size);
399  if (hasColor)
400  {
401  if (monoImage_arg)
402  {
403  assert (rgbData_arg.size()==cloud_size);
404  } else
405  {
406  assert (rgbData_arg.size()==cloud_size*3);
407  }
408  }
409 
410  int x, y, centerX, centerY;
411 
412  // Reset point cloud
413  cloud_arg.points.clear();
414  cloud_arg.points.reserve(cloud_size);
415 
416  // Define point cloud parameters
417  cloud_arg.width = static_cast<uint32_t>(width_arg);
418  cloud_arg.height = static_cast<uint32_t>(height_arg);
419  cloud_arg.is_dense = false;
420 
421  // Calculate center of disparity image
422  centerX = static_cast<int>(width_arg/2);
423  centerY = static_cast<int>(height_arg/2);
424 
425  const float fl_const = 1.0f/focalLength_arg;
426  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
427 
428  i = 0;
429  for (y=-centerY; y<+centerY; ++y )
430  for (x=-centerX; x<+centerX; ++x )
431  {
432  PointT newPoint;
433 
434  const uint16_t& pixel_disparity = disparityData_arg[i];
435 
436  if (pixel_disparity && (pixel_disparity!=0x7FF))
437  {
438  float depth = focalLength_arg / (static_cast<float> (pixel_disparity) * disparityScale_arg + disparityShift_arg);
439 
440  // Define point location
441  newPoint.z = depth;
442  newPoint.x = static_cast<float> (x) * depth * fl_const;
443  newPoint.y = static_cast<float> (y) * depth * fl_const;
444 
445  if (hasColor)
446  {
447  if (monoImage_arg)
448  {
449  const uint8_t& pixel_r = rgbData_arg[i];
450  const uint8_t& pixel_g = rgbData_arg[i];
451  const uint8_t& pixel_b = rgbData_arg[i];
452 
453  // Define point color
454  uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16
455  | static_cast<uint32_t>(pixel_g) << 8
456  | static_cast<uint32_t>(pixel_b));
457  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
458  } else
459  {
460  const uint8_t& pixel_r = rgbData_arg[i*3+0];
461  const uint8_t& pixel_g = rgbData_arg[i*3+1];
462  const uint8_t& pixel_b = rgbData_arg[i*3+2];
463 
464  // Define point color
465  uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16
466  | static_cast<uint32_t>(pixel_g) << 8
467  | static_cast<uint32_t>(pixel_b));
468  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
469  }
470 
471  } else
472  {
473  // Set white point color
474  uint32_t rgb = (static_cast<uint32_t>(255) << 16
475  | static_cast<uint32_t>(255) << 8
476  | static_cast<uint32_t>(255));
477  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
478  }
479  } else
480  {
481  // Define bad point
482  newPoint.x = newPoint.y = newPoint.z = bad_point;
483  newPoint.rgb = 0.0f;
484  }
485 
486  // Add point to cloud
487  cloud_arg.points.push_back(newPoint);
488  // Increment point iterator
489  ++i;
490  }
491  }
492 
493  /** \brief Convert disparity image to point cloud
494  * \param[in] depthData_arg output disparity image
495  * \param[in] rgbData_arg output rgb image
496  * \param[in] monoImage_arg input image is a single-channel mono image
497  * \param[in] width_arg width of disparity image
498  * \param[in] height_arg height of disparity image
499  * \param[in] focalLength_arg focal length
500  * \param[out] cloud_arg output point cloud
501  * \ingroup io
502  */
503  static void convert(typename std::vector<float>& depthData_arg,
504  typename std::vector<uint8_t>& rgbData_arg,
505  bool monoImage_arg,
506  size_t width_arg,
507  size_t height_arg,
508  float focalLength_arg,
509  pcl::PointCloud<PointT>& cloud_arg)
510  {
511  size_t i;
512  size_t cloud_size = width_arg*height_arg;
513  bool hasColor = (rgbData_arg.size () > 0);
514 
515  // Check size of input data
516  assert (depthData_arg.size()==cloud_size);
517  if (hasColor)
518  {
519  if (monoImage_arg)
520  {
521  assert (rgbData_arg.size()==cloud_size);
522  } else
523  {
524  assert (rgbData_arg.size()==cloud_size*3);
525  }
526  }
527 
528  int x, y, centerX, centerY;
529 
530  // Reset point cloud
531  cloud_arg.points.clear();
532  cloud_arg.points.reserve(cloud_size);
533 
534  // Define point cloud parameters
535  cloud_arg.width = static_cast<uint32_t>(width_arg);
536  cloud_arg.height = static_cast<uint32_t>(height_arg);
537  cloud_arg.is_dense = false;
538 
539  // Calculate center of disparity image
540  centerX = static_cast<int>(width_arg/2);
541  centerY = static_cast<int>(height_arg/2);
542 
543  const float fl_const = 1.0f/focalLength_arg;
544  static const float bad_point = std::numeric_limits<float>::quiet_NaN ();
545 
546  i = 0;
547  for (y=-centerY; y<+centerY; ++y )
548  for (x=-centerX; x<+centerX; ++x )
549  {
550  PointT newPoint;
551 
552  const float& pixel_depth = depthData_arg[i];
553 
554  if (pixel_depth==pixel_depth)
555  {
556  float depth = focalLength_arg / pixel_depth;
557 
558  // Define point location
559  newPoint.z = depth;
560  newPoint.x = static_cast<float> (x) * depth * fl_const;
561  newPoint.y = static_cast<float> (y) * depth * fl_const;
562 
563  if (hasColor)
564  {
565  if (monoImage_arg)
566  {
567  const uint8_t& pixel_r = rgbData_arg[i];
568  const uint8_t& pixel_g = rgbData_arg[i];
569  const uint8_t& pixel_b = rgbData_arg[i];
570 
571  // Define point color
572  uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16
573  | static_cast<uint32_t>(pixel_g) << 8
574  | static_cast<uint32_t>(pixel_b));
575  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
576  } else
577  {
578  const uint8_t& pixel_r = rgbData_arg[i*3+0];
579  const uint8_t& pixel_g = rgbData_arg[i*3+1];
580  const uint8_t& pixel_b = rgbData_arg[i*3+2];
581 
582  // Define point color
583  uint32_t rgb = (static_cast<uint32_t>(pixel_r) << 16
584  | static_cast<uint32_t>(pixel_g) << 8
585  | static_cast<uint32_t>(pixel_b));
586  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
587  }
588 
589  } else
590  {
591  // Set white point color
592  uint32_t rgb = (static_cast<uint32_t>(255) << 16
593  | static_cast<uint32_t>(255) << 8
594  | static_cast<uint32_t>(255));
595  newPoint.rgb = *reinterpret_cast<float*>(&rgb);
596  }
597  } else
598  {
599  // Define bad point
600  newPoint.x = newPoint.y = newPoint.z = bad_point;
601  newPoint.rgb = 0.0f;
602  }
603 
604  // Add point to cloud
605  cloud_arg.points.push_back(newPoint);
606  // Increment point iterator
607  ++i;
608  }
609  }
610  };
611 
612  }
613 }
614 
615 
616 #endif