Point Cloud Library (PCL)  1.14.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 #include <pcl/common/colors.h> // for RGB2sRGB_LUT
47 
48 namespace pcl
49 {
50  // r,g,b, i values are from 0 to 255
51  // h = [0,360]
52  // s, v values are from 0 to 1
53  // if s = 0 => h = 0
54 
55  /** \brief Convert a XYZRGB point type to a XYZI
56  * \param[in] in the input XYZRGB point
57  * \param[out] out the output XYZI point
58  */
59  inline void
61  PointXYZI& out)
62  {
63  out.x = in.x; out.y = in.y; out.z = in.z;
64  out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
65  }
66 
67  /** \brief Convert a RGB point type to a I
68  * \param[in] in the input RGB point
69  * \param[out] out the output Intensity point
70  */
71  inline void
72  PointRGBtoI (const RGB& in,
73  Intensity& out)
74  {
75  out.intensity = 0.299f * static_cast <float> (in.r) + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b);
76  }
77 
78  /** \brief Convert a RGB point type to a I
79  * \param[in] in the input RGB point
80  * \param[out] out the output Intensity point
81  */
82  inline void
83  PointRGBtoI (const RGB& in,
84  Intensity8u& out)
85  {
86  out.intensity = static_cast<std::uint8_t>(0.299f * static_cast <float> (in.r)
87  + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
88  }
89 
90  /** \brief Convert a RGB point type to a I
91  * \param[in] in the input RGB point
92  * \param[out] out the output Intensity point
93  */
94  inline void
95  PointRGBtoI (const RGB& in,
96  Intensity32u& out)
97  {
98  out.intensity = static_cast<std::uint32_t>(0.299f * static_cast <float> (in.r)
99  + 0.587f * static_cast <float> (in.g) + 0.114f * static_cast <float> (in.b));
100  }
101 
102  /** \brief Convert a XYZRGB point type to a XYZHSV
103  * \param[in] in the input XYZRGB point
104  * \param[out] out the output XYZHSV point
105  */
106  inline void
108  PointXYZHSV& out)
109  {
110  const unsigned char max = std::max (in.r, std::max (in.g, in.b));
111  const unsigned char min = std::min (in.r, std::min (in.g, in.b));
112 
113  out.x = in.x; out.y = in.y; out.z = in.z;
114  out.v = static_cast <float> (max) / 255.f;
115 
116  if (max == 0) // division by zero
117  {
118  out.s = 0.f;
119  out.h = 0.f; // h = -1.f;
120  return;
121  }
122 
123  const float diff = static_cast <float> (max - min);
124  out.s = diff / static_cast <float> (max);
125 
126  if (min == max) // diff == 0 -> division by zero
127  {
128  out.h = 0;
129  return;
130  }
131 
132  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
133  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
134  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
135 
136  if (out.h < 0.f) out.h += 360.f;
137  }
138 
139  /** \brief Convert a XYZRGB-based point type to a XYZLAB
140  * \param[in] in the input XYZRGB(XYZRGBA, XYZRGBL, etc.) point
141  * \param[out] out the output XYZLAB point
142  */
143  template <typename PointT, traits::HasColor<PointT> = true>
144  inline void
146  PointXYZLAB& out)
147  {
148  out.x = in.x;
149  out.y = in.y;
150  out.z = in.z;
151  out.data[3] = 1.0; // important for homogeneous coordinates
152 
153  // convert sRGB to CIELAB
154  // for sRGB -> CIEXYZ see http://www.easyrgb.com/index.php?X=MATH&H=02#text2
155  // for CIEXYZ -> CIELAB see http://www.easyrgb.com/index.php?X=MATH&H=07#text7
156  // an overview at: https://www.comp.nus.edu.sg/~leowwk/papers/colordiff.pdf
157 
158  const auto& sRGB_LUT = RGB2sRGB_LUT<double, 8>();
159 
160  const double R = sRGB_LUT[in.r];
161  const double G = sRGB_LUT[in.g];
162  const double B = sRGB_LUT[in.b];
163 
164  // linear sRGB -> CIEXYZ, D65 illuminant, observer at 2 degrees
165  const double X = R * 0.4124 + G * 0.3576 + B * 0.1805;
166  const double Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
167  const double Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
168 
169  // normalize X, Y, Z with tristimulus values for Xn, Yn, Zn
170  float f[3] = {static_cast<float>(X), static_cast<float>(Y), static_cast<float>(Z)};
171  f[0] /= 0.95047;
172  f[1] /= 1;
173  f[2] /= 1.08883;
174 
175  // CIEXYZ -> CIELAB
176  for (float & xyz : f) {
177  if (xyz > 0.008856) {
178  xyz = std::pow(xyz, 1.0 / 3.0);
179  }
180  else {
181  xyz = 7.787 * xyz + 16.0 / 116.0;
182  }
183  }
184 
185  out.L = 116.0f * f[1] - 16.0f;
186  out.a = 500.0f * (f[0] - f[1]);
187  out.b = 200.0f * (f[1] - f[2]);
188  }
189 
190  /** \brief Convert a XYZRGBA point type to a XYZHSV
191  * \param[in] in the input XYZRGBA point
192  * \param[out] out the output XYZHSV point
193  * \todo include the A parameter but how?
194  */
195  inline void
197  PointXYZHSV& out)
198  {
199  const unsigned char max = std::max (in.r, std::max (in.g, in.b));
200  const unsigned char min = std::min (in.r, std::min (in.g, in.b));
201 
202  out.x = in.x; out.y = in.y; out.z = in.z;
203  out.v = static_cast <float> (max) / 255.f;
204 
205  if (max == 0) // division by zero
206  {
207  out.s = 0.f;
208  out.h = 0.f;
209  return;
210  }
211 
212  const float diff = static_cast <float> (max - min);
213  out.s = diff / static_cast <float> (max);
214 
215  if (min == max) // diff == 0 -> division by zero
216  {
217  out.h = 0;
218  return;
219  }
220 
221  if (max == in.r) out.h = 60.f * ( static_cast <float> (in.g - in.b) / diff);
222  else if (max == in.g) out.h = 60.f * (2.f + static_cast <float> (in.b - in.r) / diff);
223  else out.h = 60.f * (4.f + static_cast <float> (in.r - in.g) / diff); // max == b
224 
225  if (out.h < 0.f) out.h += 360.f;
226  }
227 
228  /* \brief Convert a XYZHSV point type to a XYZRGB
229  * \param[in] in the input XYZHSV point
230  * \param[out] out the output XYZRGB point
231  */
232  inline void
234  PointXYZRGB& out)
235  {
236  out.x = in.x; out.y = in.y; out.z = in.z;
237  if (in.s == 0)
238  {
239  out.r = out.g = out.b = static_cast<std::uint8_t> (255 * in.v);
240  return;
241  }
242  float a = in.h / 60;
243  int i = static_cast<int> (std::floor (a));
244  float f = a - static_cast<float> (i);
245  float p = in.v * (1 - in.s);
246  float q = in.v * (1 - in.s * f);
247  float t = in.v * (1 - in.s * (1 - f));
248 
249  switch (i)
250  {
251  case 0:
252  {
253  out.r = static_cast<std::uint8_t> (255 * in.v);
254  out.g = static_cast<std::uint8_t> (255 * t);
255  out.b = static_cast<std::uint8_t> (255 * p);
256  break;
257  }
258  case 1:
259  {
260  out.r = static_cast<std::uint8_t> (255 * q);
261  out.g = static_cast<std::uint8_t> (255 * in.v);
262  out.b = static_cast<std::uint8_t> (255 * p);
263  break;
264  }
265  case 2:
266  {
267  out.r = static_cast<std::uint8_t> (255 * p);
268  out.g = static_cast<std::uint8_t> (255 * in.v);
269  out.b = static_cast<std::uint8_t> (255 * t);
270  break;
271  }
272  case 3:
273  {
274  out.r = static_cast<std::uint8_t> (255 * p);
275  out.g = static_cast<std::uint8_t> (255 * q);
276  out.b = static_cast<std::uint8_t> (255 * in.v);
277  break;
278  }
279  case 4:
280  {
281  out.r = static_cast<std::uint8_t> (255 * t);
282  out.g = static_cast<std::uint8_t> (255 * p);
283  out.b = static_cast<std::uint8_t> (255 * in.v);
284  break;
285  }
286  default:
287  {
288  out.r = static_cast<std::uint8_t> (255 * in.v);
289  out.g = static_cast<std::uint8_t> (255 * p);
290  out.b = static_cast<std::uint8_t> (255 * q);
291  break;
292  }
293  }
294  }
295 
296  /** \brief Convert a RGB point cloud to an Intensity
297  * \param[in] in the input RGB point cloud
298  * \param[out] out the output Intensity point cloud
299  */
300  inline void
303  {
304  out.width = in.width;
305  out.height = in.height;
306  for (const auto &point : in)
307  {
308  Intensity p;
309  PointRGBtoI (point, p);
310  out.push_back (p);
311  }
312  }
313 
314  /** \brief Convert a RGB point cloud to an Intensity
315  * \param[in] in the input RGB point cloud
316  * \param[out] out the output Intensity point cloud
317  */
318  inline void
321  {
322  out.width = in.width;
323  out.height = in.height;
324  for (const auto &point : in)
325  {
326  Intensity8u p;
327  PointRGBtoI (point, p);
328  out.push_back (p);
329  }
330  }
331 
332  /** \brief Convert a RGB point cloud to an Intensity
333  * \param[in] in the input RGB point cloud
334  * \param[out] out the output Intensity point cloud
335  */
336  inline void
339  {
340  out.width = in.width;
341  out.height = in.height;
342  for (const auto &point : in)
343  {
344  Intensity32u p;
345  PointRGBtoI (point, p);
346  out.push_back (p);
347  }
348  }
349 
350  /** \brief Convert a XYZRGB point cloud to a XYZHSV
351  * \param[in] in the input XYZRGB point cloud
352  * \param[out] out the output XYZHSV point cloud
353  */
354  inline void
357  {
358  out.width = in.width;
359  out.height = in.height;
360  for (const auto &point : in)
361  {
362  PointXYZHSV p;
363  PointXYZRGBtoXYZHSV (point, p);
364  out.push_back (p);
365  }
366  }
367 
368  /** \brief Convert a XYZHSV point cloud to a XYZRGB
369  * \param[in] in the input XYZHSV point cloud
370  * \param[out] out the output XYZRGB point cloud
371  */
372  inline void
375  {
376  out.width = in.width;
377  out.height = in.height;
378  for (const auto &point : in)
379  {
380  PointXYZRGB p;
381  PointXYZHSVtoXYZRGB (point, p);
382  out.push_back (p);
383  }
384  }
385 
386  /** \brief Convert a XYZRGB point cloud to a XYZHSV
387  * \param[in] in the input XYZRGB point cloud
388  * \param[out] out the output XYZHSV point cloud
389  */
390  inline void
393  {
394  out.width = in.width;
395  out.height = in.height;
396  for (const auto &point : in)
397  {
398  PointXYZHSV p;
399  PointXYZRGBAtoXYZHSV (point, p);
400  out.push_back (p);
401  }
402  }
403 
404  /** \brief Convert a XYZRGB point cloud to a XYZI
405  * \param[in] in the input XYZRGB point cloud
406  * \param[out] out the output XYZI point cloud
407  */
408  inline void
411  {
412  out.width = in.width;
413  out.height = in.height;
414  for (const auto &point : in)
415  {
416  PointXYZI p;
417  PointXYZRGBtoXYZI (point, p);
418  out.push_back (p);
419  }
420  }
421 
422  /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA
423  * \param[in] depth the input depth image as intensity points in float
424  * \param[in] image the input RGB image
425  * \param[in] focal the focal length
426  * \param[out] out the output pointcloud
427  * **/
428  inline void
430  const PointCloud<RGB>& image,
431  const float& focal,
433  {
434  float bad_point = std::numeric_limits<float>::quiet_NaN();
435  std::size_t width_ = depth.width;
436  std::size_t height_ = depth.height;
437  float constant_ = 1.0f / focal;
438 
439  for (std::size_t v = 0; v < height_; v++)
440  {
441  for (std::size_t u = 0; u < width_; u++)
442  {
443  PointXYZRGBA pt;
444  float depth_ = depth.at (u, v).intensity;
445 
446  if (depth_ == 0)
447  {
448  pt.x = pt.y = pt.z = bad_point;
449  }
450  else
451  {
452  pt.z = depth_ * 0.001f;
453  pt.x = static_cast<float> (u) * pt.z * constant_;
454  pt.y = static_cast<float> (v) * pt.z * constant_;
455  }
456  pt.r = image.at (u, v).r;
457  pt.g = image.at (u, v).g;
458  pt.b = image.at (u, v).b;
459 
460  out.push_back (pt);
461  }
462  }
463  out.width = width_;
464  out.height = height_;
465  }
466 }
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
Defines all the PCL implemented PointT point type structures.
@ B
Definition: norms.h:54
void PointXYZRGBtoXYZLAB(const PointT &in, PointXYZLAB &out)
Convert a XYZRGB-based point type to a XYZLAB.
void PointCloudXYZHSVtoXYZRGB(const PointCloud< PointXYZHSV > &in, PointCloud< PointXYZRGB > &out)
Convert a XYZHSV point cloud to a XYZRGB.
void PointCloudRGBtoI(const PointCloud< RGB > &in, PointCloud< Intensity > &out)
Convert a RGB point cloud to an Intensity.
void PointCloudXYZRGBAtoXYZHSV(const PointCloud< PointXYZRGBA > &in, PointCloud< PointXYZHSV > &out)
Convert a XYZRGB point cloud to a XYZHSV.
void PointXYZRGBtoXYZHSV(const PointXYZRGB &in, PointXYZHSV &out)
Convert a XYZRGB point type to a XYZHSV.
void PointXYZHSVtoXYZRGB(const PointXYZHSV &in, PointXYZRGB &out)
void PointXYZRGBAtoXYZHSV(const PointXYZRGBA &in, PointXYZHSV &out)
Convert a XYZRGBA point type to a XYZHSV.
void PointRGBtoI(const RGB &in, Intensity &out)
Convert a RGB point type to a I.
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 PointXYZRGBtoXYZI(const PointXYZRGB &in, PointXYZI &out)
Convert a XYZRGB point type to a XYZI.
void PointCloudXYZRGBtoXYZHSV(const PointCloud< PointXYZRGB > &in, PointCloud< PointXYZHSV > &out)
Convert a XYZRGB point cloud to a XYZHSV.
void PointCloudXYZRGBtoXYZI(const PointCloud< PointXYZRGB > &in, PointCloud< PointXYZI > &out)
Convert a XYZRGB point cloud to a XYZI.
A point structure representing the grayscale intensity in single-channel images.
A point structure representing the grayscale intensity in single-channel images.
A point structure representing the grayscale intensity in single-channel images.
A point structure representing Euclidean xyz coordinates, and the CIELAB color.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.