Point Cloud Library (PCL)  1.9.1-dev
point_types_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 the copyright holder(s) 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  */
38 
39 #pragma once
40 
41 #include <limits>
42 
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 
46 namespace pcl
47 {
48  // r,g,b, i values are from 0 to 255
49  // h = [0,360]
50  // s, v values are from 0 to 1
51  // if s = 0 => h = 0
52 
53  /** \brief Convert a XYZRGB point type to a XYZI
54  * \param[in] in the input XYZRGB point
55  * \param[out] out the output XYZI point
56  */
57  inline void
59  PointXYZI& out)
60  {
61  out.x = in.x; out.y = in.y; out.z = in.z;
62  out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
63  }
64 
65  /** \brief Convert a RGB point type to a I
66  * \param[in] in the input RGB point
67  * \param[out] out the output Intensity point
68  */
69  inline void
70  PointRGBtoI (const RGB& in,
71  Intensity& out)
72  {
73  out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
74  }
75 
76  /** \brief Convert a RGB point type to a I
77  * \param[in] in the input RGB point
78  * \param[out] out the output Intensity point
79  */
80  inline void
81  PointRGBtoI (const RGB& in,
82  Intensity8u& out)
83  {
84  out.intensity = static_cast<uint8_t>(0.299f * static_cast <float> (in.r)
85  + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
86  }
87 
88  /** \brief Convert a RGB point type to a I
89  * \param[in] in the input RGB point
90  * \param[out] out the output Intensity point
91  */
92  inline void
93  PointRGBtoI (const RGB& in,
94  Intensity32u& out)
95  {
96  out.intensity = static_cast<uint32_t>(0.299f * static_cast <float> (in.r)
97  + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
98  }
99 
100  /** \brief Convert a XYZRGB point type to a XYZHSV
101  * \param[in] in the input XYZRGB point
102  * \param[out] out the output XYZHSV point
103  */
104  inline void
106  PointXYZHSV& out)
107  {
108  const unsigned char max = std::max (in.r, std::max (in.g, in.b));
109  const unsigned char min = std::min (in.r, std::min (in.g, in.b));
110 
111  out.x = in.x; out.y = in.y; out.z = in.z;
112  out.v = static_cast <float> (max) / 255.f;
113 
114  if (max == 0) // division by zero
115  {
116  out.s = 0.f;
117  out.h = 0.f; // h = -1.f;
118  return;
119  }
120 
121  const float diff = static_cast <float> (max - min);
122  out.s = diff / static_cast <float> (max);
123 
124  if (min == max) // diff == 0 -> division by zero
125  {
126  out.h = 0;
127  return;
128  }
129 
130  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
131  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
132  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
133 
134  if (out.h < 0.f) out.h += 360.f;
135  }
136 
137  /** \brief Convert a XYZRGBA point type to a XYZHSV
138  * \param[in] in the input XYZRGBA point
139  * \param[out] out the output XYZHSV point
140  * \todo include the A parameter but how?
141  */
142  inline void
144  PointXYZHSV& out)
145  {
146  const unsigned char max = std::max (in.r, std::max (in.g, in.b));
147  const unsigned char min = std::min (in.r, std::min (in.g, in.b));
148 
149  out.x = in.x; out.y = in.y; out.z = in.z;
150  out.v = static_cast <float> (max) / 255.f;
151 
152  if (max == 0) // division by zero
153  {
154  out.s = 0.f;
155  out.h = 0.f;
156  return;
157  }
158 
159  const float diff = static_cast <float> (max - min);
160  out.s = diff / static_cast <float> (max);
161 
162  if (min == max) // diff == 0 -> division by zero
163  {
164  out.h = 0;
165  return;
166  }
167 
168  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
169  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
170  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
171 
172  if (out.h < 0.f) out.h += 360.f;
173  }
174 
175  /* \brief Convert a XYZHSV point type to a XYZRGB
176  * \param[in] in the input XYZHSV point
177  * \param[out] out the output XYZRGB point
178  */
179  inline void
181  PointXYZRGB& out)
182  {
183  out.x = in.x; out.y = in.y; out.z = in.z;
184  if (in.s == 0)
185  {
186  out.r = out.g = out.b = static_cast<uint8_t> (255 * in.v);
187  return;
188  }
189  float a = in.h / 60;
190  int i = static_cast<int> (std::floor (a));
191  float f = a - static_cast<float> (i);
192  float p = in.v * (1 - in.s);
193  float q = in.v * (1 - in.s * f);
194  float t = in.v * (1 - in.s * (1 - f));
195 
196  switch (i)
197  {
198  case 0:
199  {
200  out.r = static_cast<uint8_t> (255 * in.v);
201  out.g = static_cast<uint8_t> (255 * t);
202  out.b = static_cast<uint8_t> (255 * p);
203  break;
204  }
205  case 1:
206  {
207  out.r = static_cast<uint8_t> (255 * q);
208  out.g = static_cast<uint8_t> (255 * in.v);
209  out.b = static_cast<uint8_t> (255 * p);
210  break;
211  }
212  case 2:
213  {
214  out.r = static_cast<uint8_t> (255 * p);
215  out.g = static_cast<uint8_t> (255 * in.v);
216  out.b = static_cast<uint8_t> (255 * t);
217  break;
218  }
219  case 3:
220  {
221  out.r = static_cast<uint8_t> (255 * p);
222  out.g = static_cast<uint8_t> (255 * q);
223  out.b = static_cast<uint8_t> (255 * in.v);
224  break;
225  }
226  case 4:
227  {
228  out.r = static_cast<uint8_t> (255 * t);
229  out.g = static_cast<uint8_t> (255 * p);
230  out.b = static_cast<uint8_t> (255 * in.v);
231  break;
232  }
233  default:
234  {
235  out.r = static_cast<uint8_t> (255 * in.v);
236  out.g = static_cast<uint8_t> (255 * p);
237  out.b = static_cast<uint8_t> (255 * q);
238  break;
239  }
240  }
241  }
242 
243  /** \brief Convert a RGB point cloud to an Intensity
244  * \param[in] in the input RGB point cloud
245  * \param[out] out the output Intensity point cloud
246  */
247  inline void
250  {
251  out.width = in.width;
252  out.height = in.height;
253  for (const auto &point : in.points)
254  {
255  Intensity p;
256  PointRGBtoI (point, p);
257  out.points.push_back (p);
258  }
259  }
260 
261  /** \brief Convert a RGB point cloud to an Intensity
262  * \param[in] in the input RGB point cloud
263  * \param[out] out the output Intensity point cloud
264  */
265  inline void
268  {
269  out.width = in.width;
270  out.height = in.height;
271  for (const auto &point : in.points)
272  {
273  Intensity8u p;
274  PointRGBtoI (point, p);
275  out.points.push_back (p);
276  }
277  }
278 
279  /** \brief Convert a RGB point cloud to an Intensity
280  * \param[in] in the input RGB point cloud
281  * \param[out] out the output Intensity point cloud
282  */
283  inline void
286  {
287  out.width = in.width;
288  out.height = in.height;
289  for (const auto &point : in.points)
290  {
291  Intensity32u p;
292  PointRGBtoI (point, p);
293  out.points.push_back (p);
294  }
295  }
296 
297  /** \brief Convert a XYZRGB point cloud to a XYZHSV
298  * \param[in] in the input XYZRGB point cloud
299  * \param[out] out the output XYZHSV point cloud
300  */
301  inline void
304  {
305  out.width = in.width;
306  out.height = in.height;
307  for (const auto &point : in.points)
308  {
309  PointXYZHSV p;
310  PointXYZRGBtoXYZHSV (point, p);
311  out.points.push_back (p);
312  }
313  }
314 
315  /** \brief Convert a XYZRGB point cloud to a XYZHSV
316  * \param[in] in the input XYZRGB point cloud
317  * \param[out] out the output XYZHSV point cloud
318  */
319  inline void
322  {
323  out.width = in.width;
324  out.height = in.height;
325  for (const auto &point : in.points)
326  {
327  PointXYZHSV p;
328  PointXYZRGBAtoXYZHSV (point, p);
329  out.points.push_back (p);
330  }
331  }
332 
333  /** \brief Convert a XYZRGB point cloud to a XYZI
334  * \param[in] in the input XYZRGB point cloud
335  * \param[out] out the output XYZI point cloud
336  */
337  inline void
340  {
341  out.width = in.width;
342  out.height = in.height;
343  for (const auto &point : in.points)
344  {
345  PointXYZI p;
346  PointXYZRGBtoXYZI (point, p);
347  out.points.push_back (p);
348  }
349  }
350 
351  /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA
352  * \param[in] depth the input depth image as intensity points in float
353  * \param[in] image the input RGB image
354  * \param[in] focal the focal length
355  * \param[out] out the output pointcloud
356  * **/
357  inline void
359  const PointCloud<RGB>& image,
360  const float& focal,
362  {
363  float bad_point = std::numeric_limits<float>::quiet_NaN();
364  size_t width_ = depth.width;
365  size_t height_ = depth.height;
366  float constant_ = 1.0f / focal;
367 
368  for (size_t v = 0; v < height_; v++)
369  {
370  for (size_t u = 0; u < width_; u++)
371  {
372  PointXYZRGBA pt;
373  float depth_ = depth.at (u, v).intensity;
374 
375  if (depth_ == 0)
376  {
377  pt.x = pt.y = pt.z = bad_point;
378  }
379  else
380  {
381  pt.z = depth_ * 0.001f;
382  pt.x = static_cast<float> (u) * pt.z * constant_;
383  pt.y = static_cast<float> (v) * pt.z * constant_;
384  }
385  pt.r = image.at (u, v).r;
386  pt.g = image.at (u, v).g;
387  pt.b = image.at (u, v).b;
388 
389  out.points.push_back (pt);
390  }
391  }
392  out.width = width_;
393  out.height = height_;
394  }
395 }
void PointXYZRGBtoXYZI(const PointXYZRGB &in, PointXYZI &out)
Convert a XYZRGB point type to a XYZI.
A point structure representing the grayscale intensity in single-channel images.
void PointCloudXYZRGBAtoXYZHSV(const PointCloud< PointXYZRGBA > &in, PointCloud< PointXYZHSV > &out)
Convert a XYZRGB point cloud to a XYZHSV.
void PointCloudXYZRGBtoXYZHSV(const PointCloud< PointXYZRGB > &in, PointCloud< PointXYZHSV > &out)
Convert a XYZRGB point cloud to a XYZHSV.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
void PointCloudDepthAndRGBtoXYZRGBA(const PointCloud< Intensity > &depth, const PointCloud< RGB > &image, const float &focal, PointCloud< PointXYZRGBA > &out)
Convert registered Depth image and RGB image to PointCloudXYZRGBA.
void PointCloudXYZRGBtoXYZI(const PointCloud< PointXYZRGB > &in, PointCloud< PointXYZI > &out)
Convert a XYZRGB point cloud to a XYZI.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing the grayscale intensity in single-channel images.
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:296
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
void PointRGBtoI(const RGB &in, Intensity &out)
Convert a RGB point type to a I.
void PointXYZRGBtoXYZHSV(const PointXYZRGB &in, PointXYZHSV &out)
Convert a XYZRGB point type to a XYZHSV.
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
void PointCloudRGBtoI(const PointCloud< RGB > &in, PointCloud< Intensity > &out)
Convert a RGB point cloud to an Intensity.
Defines all the PCL implemented PointT point type structures.
void PointXYZRGBAtoXYZHSV(const PointXYZRGBA &in, PointXYZHSV &out)
Convert a XYZRGBA point type to a XYZHSV.
A point structure representing the grayscale intensity in single-channel images.
void PointXYZHSVtoXYZRGB(const PointXYZHSV &in, PointXYZRGB &out)
A point structure representing Euclidean xyz coordinates, and the RGB color.