Point Cloud Library (PCL) 1.12.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#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
48namespace 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 (int i = 0; i < 3; ++i) {
177 if (f[i] > 0.008856) {
178 f[i] = std::pow(f[i], 1.0 / 3.0);
179 }
180 else {
181 f[i] = 7.787 * f[i] + 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.points)
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.points)
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.points)
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.points)
361 {
362 PointXYZHSV p;
363 PointXYZRGBtoXYZHSV (point, p);
364 out.push_back (p);
365 }
366 }
367
368 /** \brief Convert a XYZRGB point cloud to a XYZHSV
369 * \param[in] in the input XYZRGB point cloud
370 * \param[out] out the output XYZHSV point cloud
371 */
372 inline void
375 {
376 out.width = in.width;
377 out.height = in.height;
378 for (const auto &point : in.points)
379 {
380 PointXYZHSV p;
381 PointXYZRGBAtoXYZHSV (point, p);
382 out.push_back (p);
383 }
384 }
385
386 /** \brief Convert a XYZRGB point cloud to a XYZI
387 * \param[in] in the input XYZRGB point cloud
388 * \param[out] out the output XYZI point cloud
389 */
390 inline void
393 {
394 out.width = in.width;
395 out.height = in.height;
396 for (const auto &point : in.points)
397 {
398 PointXYZI p;
399 PointXYZRGBtoXYZI (point, p);
400 out.push_back (p);
401 }
402 }
403
404 /** \brief Convert registered Depth image and RGB image to PointCloudXYZRGBA
405 * \param[in] depth the input depth image as intensity points in float
406 * \param[in] image the input RGB image
407 * \param[in] focal the focal length
408 * \param[out] out the output pointcloud
409 * **/
410 inline void
412 const PointCloud<RGB>& image,
413 const float& focal,
415 {
416 float bad_point = std::numeric_limits<float>::quiet_NaN();
417 std::size_t width_ = depth.width;
418 std::size_t height_ = depth.height;
419 float constant_ = 1.0f / focal;
420
421 for (std::size_t v = 0; v < height_; v++)
422 {
423 for (std::size_t u = 0; u < width_; u++)
424 {
425 PointXYZRGBA pt;
426 float depth_ = depth.at (u, v).intensity;
427
428 if (depth_ == 0)
429 {
430 pt.x = pt.y = pt.z = bad_point;
431 }
432 else
433 {
434 pt.z = depth_ * 0.001f;
435 pt.x = static_cast<float> (u) * pt.z * constant_;
436 pt.y = static_cast<float> (v) * pt.z * constant_;
437 }
438 pt.r = image.at (u, v).r;
439 pt.g = image.at (u, v).g;
440 pt.b = image.at (u, v).b;
441
442 out.push_back (pt);
443 }
444 }
445 out.width = width_;
446 out.height = height_;
447 }
448}
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
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 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.