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