Point Cloud Library (PCL)  1.7.0
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
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
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
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.v = static_cast <float> (max) / 255.f;
110 
111  if (max == 0) // division by zero
112  {
113  out.s = 0.f;
114  out.h = 0.f; // h = -1.f;
115  return;
116  }
117 
118  const float diff = static_cast <float> (max - min);
119  out.s = diff / static_cast <float> (max);
120 
121  if (min == max) // diff == 0 -> division by zero
122  {
123  out.h = 0;
124  return;
125  }
126 
127  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
128  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
129  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
130 
131  if (out.h < 0.f) out.h += 360.f;
132  }
133 
134  /** \brief Convert a XYZRGB point type to a XYZHSV
135  * \param[in] in the input XYZRGB point
136  * \param[out] out the output XYZHSV point
137  * \todo include the A parameter but how?
138  */
139  inline void
141  PointXYZHSV& out)
142  {
143  const unsigned char max = std::max (in.r, std::max (in.g, in.b));
144  const unsigned char min = std::min (in.r, std::min (in.g, in.b));
145 
146  out.v = static_cast <float> (max) / 255.f;
147 
148  if (max == 0) // division by zero
149  {
150  out.s = 0.f;
151  out.h = 0.f; // h = -1.f;
152  return;
153  }
154 
155  const float diff = static_cast <float> (max - min);
156  out.s = diff / static_cast <float> (max);
157 
158  if (min == max) // diff == 0 -> division by zero
159  {
160  out.h = 0;
161  return;
162  }
163 
164  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
165  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
166  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
167 
168  if (out.h < 0.f) out.h += 360.f;
169  }
170 
171  /* \brief Convert a XYZHSV point type to a XYZRGB
172  * \param[in] in the input XYZHSV point
173  * \param[out] out the output XYZRGB point
174  */
175  inline void
177  PointXYZRGB& out)
178  {
179  out.x = in.x; out.y = in.y; out.z = in.z;
180  if (in.s == 0)
181  {
182  out.r = out.g = out.b = static_cast<uint8_t> (in.v);
183  return;
184  }
185  float a = in.h / 60;
186  int i = static_cast<int> (floorf (a));
187  float f = a - static_cast<float> (i);
188  float p = in.v * (1 - in.s);
189  float q = in.v * (1 - in.s * f);
190  float t = in.v * (1 - in.s * (1 - f));
191 
192  switch (i)
193  {
194  case 0:
195  {
196  out.r = static_cast<uint8_t> (255 * in.v);
197  out.g = static_cast<uint8_t> (255 * t);
198  out.b = static_cast<uint8_t> (255 * p);
199  break;
200  }
201  case 1:
202  {
203  out.r = static_cast<uint8_t> (255 * q);
204  out.g = static_cast<uint8_t> (255 * in.v);
205  out.b = static_cast<uint8_t> (255 * p);
206  break;
207  }
208  case 2:
209  {
210  out.r = static_cast<uint8_t> (255 * p);
211  out.g = static_cast<uint8_t> (255 * in.v);
212  out.b = static_cast<uint8_t> (255 * t);
213  break;
214  }
215  case 3:
216  {
217  out.r = static_cast<uint8_t> (255 * p);
218  out.g = static_cast<uint8_t> (255 * q);
219  out.b = static_cast<uint8_t> (255 * in.v);
220  break;
221  }
222  case 4:
223  {
224  out.r = static_cast<uint8_t> (255 * t);
225  out.g = static_cast<uint8_t> (255 * p);
226  out.b = static_cast<uint8_t> (255 * in.v);
227  break;
228  }
229  default:
230  {
231  out.r = static_cast<uint8_t> (255 * in.v);
232  out.g = static_cast<uint8_t> (255 * p);
233  out.b = static_cast<uint8_t> (255 * q);
234  break;
235  }
236  }
237  }
238 
239  /** \brief Convert a RGB point cloud to a Intensity
240  * \param[in] in the input RGB point cloud
241  * \param[out] out the output Intensity point cloud
242  */
243  inline void
246  {
247  out.width = in.width;
248  out.height = in.height;
249  for (size_t i = 0; i < in.points.size (); i++)
250  {
251  Intensity p;
252  PointRGBtoI (in.points[i], p);
253  out.points.push_back (p);
254  }
255  }
256 
257  /** \brief Convert a RGB point cloud to a Intensity
258  * \param[in] in the input RGB point cloud
259  * \param[out] out the output Intensity point cloud
260  */
261  inline void
264  {
265  out.width = in.width;
266  out.height = in.height;
267  for (size_t i = 0; i < in.points.size (); i++)
268  {
269  Intensity8u p;
270  PointRGBtoI (in.points[i], p);
271  out.points.push_back (p);
272  }
273  }
274 
275  /** \brief Convert a RGB point cloud to a Intensity
276  * \param[in] in the input RGB point cloud
277  * \param[out] out the output Intensity point cloud
278  */
279  inline void
282  {
283  out.width = in.width;
284  out.height = in.height;
285  for (size_t i = 0; i < in.points.size (); i++)
286  {
287  Intensity32u p;
288  PointRGBtoI (in.points[i], p);
289  out.points.push_back (p);
290  }
291  }
292 
293  /** \brief Convert a XYZRGB point cloud to a XYZHSV
294  * \param[in] in the input XYZRGB point cloud
295  * \param[out] out the output XYZHSV point cloud
296  */
297  inline void
300  {
301  out.width = in.width;
302  out.height = in.height;
303  for (size_t i = 0; i < in.points.size (); i++)
304  {
305  PointXYZHSV p;
306  PointXYZRGBtoXYZHSV (in.points[i], p);
307  out.points.push_back (p);
308  }
309  }
310 
311  /** \brief Convert a XYZRGB point cloud to a XYZHSV
312  * \param[in] in the input XYZRGB point cloud
313  * \param[out] out the output XYZHSV point cloud
314  */
315  inline void
318  {
319  out.width = in.width;
320  out.height = in.height;
321  for (size_t i = 0; i < in.points.size (); i++)
322  {
323  PointXYZHSV p;
324  PointXYZRGBAtoXYZHSV (in.points[i], p);
325  out.points.push_back (p);
326  }
327  }
328 
329  /** \brief Convert a XYZRGB point cloud to a XYZI
330  * \param[in] in the input XYZRGB point cloud
331  * \param[out] out the output XYZI point cloud
332  */
333  inline void
336  {
337  out.width = in.width;
338  out.height = in.height;
339  for (size_t i = 0; i < in.points.size (); i++)
340  {
341  PointXYZI p;
342  PointXYZRGBtoXYZI (in.points[i], p);
343  out.points.push_back (p);
344  }
345  }
346 
347  /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA
348  * \param[in] depth the input depth image as intensity points in float
349  * \param[in] image the input RGB image
350  * \param[in] focal the focal length
351  * \param[out] out the output pointcloud
352  * **/
353  void
355  PointCloud<RGB>& image,
356  float& focal,
358  {
359  float bad_point = std::numeric_limits<float>::quiet_NaN();
360  int width_ = depth.width;
361  int height_ = depth.height;
362  float constant_ = 1.0f / focal;
363 
364  for(unsigned int v = 0; v < height_; v++)
365  {
366  for(unsigned int u = 0; u < width_; u++)
367  {
368  PointXYZRGBA pt;
369  pt.a = 0;
370  float depth_ = depth.at(u,v).intensity;
371 
372  if (depth_ == 0)
373  {
374  pt.x = pt.y = pt.z = bad_point;
375  }
376  else
377  {
378  pt.z = depth_ * 0.001f;
379  pt.x = static_cast<float> (u) * pt.z * constant_;
380  pt.y = static_cast<float> (v) * pt.z * constant_;
381  }
382  pt.r = image.at(u,v).r;
383  pt.g = image.at(u,v).g;
384  pt.b = image.at(u,v).b;
385 
386  out.points.push_back(pt);
387  }
388  }
389  out.width = width_;
390  out.height = height_;
391  }
392 }
393 
394 #endif //#ifndef PCL_TYPE_CONVERSIONS_H
395