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