Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
point_cloud_color_handlers.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, 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 */
37
38#pragma once
39
40#include <set>
41#include <map>
42
43#include <pcl/pcl_macros.h>
44#include <pcl/common/colors.h>
45#include <pcl/common/io.h> // for getFieldIndex
46#include <pcl/common/point_tests.h> // for pcl::isFinite
47
48
49namespace pcl
50{
51
52namespace visualization
53{
54
55template <typename PointT> vtkSmartPointer<vtkDataArray>
57{
58 if (!capable_ || !cloud_)
59 return nullptr;
60
62 scalars->SetNumberOfComponents (3);
63
64 vtkIdType nr_points = cloud_->size ();
65 scalars->SetNumberOfTuples (nr_points);
66
67 // Get a random color
68 unsigned char* colors = new unsigned char[nr_points * 3];
69
70 // Color every point
71 for (vtkIdType cp = 0; cp < nr_points; ++cp)
72 {
73 colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
74 colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
75 colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
76 }
77 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
78 return scalars;
79}
80
81
82template <typename PointT> vtkSmartPointer<vtkDataArray>
84{
85 if (!capable_ || !cloud_)
86 return nullptr;
87
89 scalars->SetNumberOfComponents (3);
90
91 vtkIdType nr_points = cloud_->size ();
92 scalars->SetNumberOfTuples (nr_points);
93
94 // Get a random color
95 unsigned char* colors = new unsigned char[nr_points * 3];
96 double r, g, b;
98
99 int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
100 g_ = static_cast<int> (pcl_lrint (g * 255.0)),
101 b_ = static_cast<int> (pcl_lrint (b * 255.0));
102
103 // Color every point
104 for (vtkIdType cp = 0; cp < nr_points; ++cp)
105 {
106 colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
107 colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
108 colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
109 }
110 scalars->SetArray (colors, 3 * nr_points, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
111 return scalars;
112}
113
114
115template <typename PointT> void
117 const PointCloudConstPtr &cloud)
118{
120 // Handle the 24-bit packed RGB values
121 field_idx_ = pcl::getFieldIndex<PointT> ("rgb", fields_);
122 if (field_idx_ != -1)
123 {
124 capable_ = true;
125 return;
126 }
127 else
128 {
129 field_idx_ = pcl::getFieldIndex<PointT> ("rgba", fields_);
130 if (field_idx_ != -1)
131 capable_ = true;
132 else
133 capable_ = false;
134 }
135}
136
137
138template <typename PointT> vtkSmartPointer<vtkDataArray>
140{
141 if (!capable_ || !cloud_)
142 return nullptr;
143
144 // Get the RGB field index
145 std::vector<pcl::PCLPointField> fields;
146 int rgba_index = -1;
147 rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
148 if (rgba_index == -1)
149 rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
150
151 int rgba_offset = fields[rgba_index].offset;
152
154 scalars->SetNumberOfComponents (3);
155
156 vtkIdType nr_points = cloud_->size ();
157 scalars->SetNumberOfTuples (nr_points);
158 unsigned char* colors = scalars->GetPointer (0);
159
160 int j = 0;
161 // If XYZ present, check if the points are invalid
162 int x_idx = -1;
163 for (std::size_t d = 0; d < fields_.size (); ++d)
164 if (fields_[d].name == "x")
165 x_idx = static_cast<int> (d);
166
167 pcl::RGB rgb;
168 if (x_idx != -1)
169 {
170 // Color every point
171 for (vtkIdType cp = 0; cp < nr_points; ++cp)
172 {
173 // Copy the value at the specified field
174 if (!std::isfinite ((*cloud_)[cp].x) ||
175 !std::isfinite ((*cloud_)[cp].y) ||
176 !std::isfinite ((*cloud_)[cp].z))
177 continue;
178
179 memcpy (&rgb, (reinterpret_cast<const char *> (&(*cloud_)[cp])) + rgba_offset, sizeof (pcl::RGB));
180 colors[j ] = rgb.r;
181 colors[j + 1] = rgb.g;
182 colors[j + 2] = rgb.b;
183 j += 3;
184 }
185 }
186 else
187 {
188 // Color every point
189 for (vtkIdType cp = 0; cp < nr_points; ++cp)
190 {
191 int idx = static_cast<int> (cp) * 3;
192 memcpy (&rgb, (reinterpret_cast<const char *> (&(*cloud_)[cp])) + rgba_offset, sizeof (pcl::RGB));
193 colors[idx ] = rgb.r;
194 colors[idx + 1] = rgb.g;
195 colors[idx + 2] = rgb.b;
196 }
197 }
198 return scalars;
199}
200
201
202template <typename PointT>
205{
206 // Check for the presence of the "H" field
207 field_idx_ = pcl::getFieldIndex<PointT> ("h", fields_);
208 if (field_idx_ == -1)
209 {
210 capable_ = false;
211 return;
212 }
213
214 // Check for the presence of the "S" field
215 s_field_idx_ = pcl::getFieldIndex<PointT> ("s", fields_);
216 if (s_field_idx_ == -1)
217 {
218 capable_ = false;
219 return;
220 }
221
222 // Check for the presence of the "V" field
223 v_field_idx_ = pcl::getFieldIndex<PointT> ("v", fields_);
224 if (v_field_idx_ == -1)
225 {
226 capable_ = false;
227 return;
228 }
229 capable_ = true;
230}
231
232
233template <typename PointT> vtkSmartPointer<vtkDataArray>
235{
236 if (!capable_ || !cloud_)
237 return nullptr;
238
240 scalars->SetNumberOfComponents (3);
241
242 vtkIdType nr_points = cloud_->size ();
243 scalars->SetNumberOfTuples (nr_points);
244 unsigned char* colors = scalars->GetPointer (0);
245
246 int idx = 0;
247 // If XYZ present, check if the points are invalid
248 int x_idx = -1;
249
250 for (std::size_t d = 0; d < fields_.size (); ++d)
251 if (fields_[d].name == "x")
252 x_idx = static_cast<int> (d);
253
254 if (x_idx != -1)
255 {
256 // Color every point
257 for (vtkIdType cp = 0; cp < nr_points; ++cp)
258 {
259 // Copy the value at the specified field
260 if (!std::isfinite ((*cloud_)[cp].x) ||
261 !std::isfinite ((*cloud_)[cp].y) ||
262 !std::isfinite ((*cloud_)[cp].z))
263 continue;
264
265 ///@todo do this with the point_types_conversion in common, first template it!
266
267 float h = (*cloud_)[cp].h;
268 float v = (*cloud_)[cp].v;
269 float s = (*cloud_)[cp].s;
270
271 // Fill color data with HSV here:
272 // restrict the hue value to [0,360[
273 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
274
275 // restrict s and v to [0,1]
276 if (s > 1.0f) s = 1.0f;
277 if (s < 0.0f) s = 0.0f;
278 if (v > 1.0f) v = 1.0f;
279 if (v < 0.0f) v = 0.0f;
280
281 if (s == 0.0f)
282 {
283 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
284 }
285 else
286 {
287 // calculate p, q, t from HSV-values
288 float a = h / 60;
289 int i = std::floor (a);
290 float f = a - i;
291 float p = v * (1 - s);
292 float q = v * (1 - s * f);
293 float t = v * (1 - s * (1 - f));
294
295 switch (i)
296 {
297 case 0:
298 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
299 case 1:
300 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
301 case 2:
302 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
303 case 3:
304 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
305 case 4:
306 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
307 case 5:
308 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
309 }
310 }
311 idx +=3;
312 }
313 }
314 else
315 {
316 // Color every point
317 for (vtkIdType cp = 0; cp < nr_points; ++cp)
318 {
319 float h = (*cloud_)[cp].h;
320 float v = (*cloud_)[cp].v;
321 float s = (*cloud_)[cp].s;
322
323 // Fill color data with HSV here:
324 // restrict the hue value to [0,360[
325 h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
326
327 // restrict s and v to [0,1]
328 if (s > 1.0f) s = 1.0f;
329 if (s < 0.0f) s = 0.0f;
330 if (v > 1.0f) v = 1.0f;
331 if (v < 0.0f) v = 0.0f;
332
333 if (s == 0.0f)
334 {
335 colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
336 }
337 else
338 {
339 // calculate p, q, t from HSV-values
340 float a = h / 60;
341 int i = std::floor (a);
342 float f = a - i;
343 float p = v * (1 - s);
344 float q = v * (1 - s * f);
345 float t = v * (1 - s * (1 - f));
346
347 switch (i)
348 {
349 case 0:
350 colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
351 case 1:
352 colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
353 case 2:
354 colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
355 case 3:
356 colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
357 case 4:
358 colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
359 case 5:
360 colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
361 }
362 }
363 idx +=3;
364 }
365 }
366 return scalars;
367}
368
369
370template <typename PointT> void
372 const PointCloudConstPtr &cloud)
373{
375 field_idx_ = pcl::getFieldIndex<PointT> (field_name_, fields_);
376 if (field_idx_ != -1)
377 capable_ = true;
378 else
379 capable_ = false;
380}
381
382
383template <typename PointT> vtkSmartPointer<vtkDataArray>
385{
386 if (!capable_ || !cloud_)
387 return nullptr;
388
389 auto scalars = vtkSmartPointer<vtkFloatArray>::New ();
390 scalars->SetNumberOfComponents (1);
391
392 vtkIdType nr_points = cloud_->size ();
393 scalars->SetNumberOfTuples (nr_points);
394
395 using FieldList = typename pcl::traits::fieldList<PointT>::type;
396
397 float* colors = new float[nr_points];
398 float field_data;
399
400 int j = 0;
401 // If XYZ present, check if the points are invalid
402 int x_idx = -1;
403 for (std::size_t d = 0; d < fields_.size (); ++d)
404 if (fields_[d].name == "x")
405 x_idx = static_cast<int> (d);
406
407 if (x_idx != -1)
408 {
409 // Color every point
410 for (vtkIdType cp = 0; cp < nr_points; ++cp)
411 {
412 // Copy the value at the specified field
413 if (!std::isfinite ((*cloud_)[cp].x) || !std::isfinite ((*cloud_)[cp].y) || !std::isfinite ((*cloud_)[cp].z))
414 continue;
415
416 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud_)[cp]);
417 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
418
419 colors[j] = field_data;
420 j++;
421 }
422 }
423 else
424 {
425 // Color every point
426 for (vtkIdType cp = 0; cp < nr_points; ++cp)
427 {
428 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud_)[cp]);
429 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
430
431 if (!std::isfinite (field_data))
432 continue;
433
434 colors[j] = field_data;
435 j++;
436 }
437 }
438 scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
439 return scalars;
440}
441
442
443template <typename PointT> void
445 const PointCloudConstPtr &cloud)
446{
448 // Handle the 24-bit packed RGBA values
449 field_idx_ = pcl::getFieldIndex<PointT> ("rgba", fields_);
450 if (field_idx_ != -1)
451 capable_ = true;
452 else
453 capable_ = false;
454}
455
456
457template <typename PointT> vtkSmartPointer<vtkDataArray>
459{
460 if (!capable_ || !cloud_)
461 return nullptr;
462
464 scalars->SetNumberOfComponents (4);
465
466 vtkIdType nr_points = cloud_->size ();
467 scalars->SetNumberOfTuples (nr_points);
468 unsigned char* colors = scalars->GetPointer (0);
469
470 // If XYZ present, check if the points are invalid
471 int x_idx = -1;
472 for (std::size_t d = 0; d < fields_.size (); ++d)
473 if (fields_[d].name == "x")
474 x_idx = static_cast<int> (d);
475
476 if (x_idx != -1)
477 {
478 int j = 0;
479 // Color every point
480 for (vtkIdType cp = 0; cp < nr_points; ++cp)
481 {
482 // Copy the value at the specified field
483 if (!std::isfinite ((*cloud_)[cp].x) ||
484 !std::isfinite ((*cloud_)[cp].y) ||
485 !std::isfinite ((*cloud_)[cp].z))
486 continue;
487
488 colors[j ] = (*cloud_)[cp].r;
489 colors[j + 1] = (*cloud_)[cp].g;
490 colors[j + 2] = (*cloud_)[cp].b;
491 colors[j + 3] = (*cloud_)[cp].a;
492 j += 4;
493 }
494 }
495 else
496 {
497 // Color every point
498 for (vtkIdType cp = 0; cp < nr_points; ++cp)
499 {
500 int idx = static_cast<int> (cp) * 4;
501 colors[idx ] = (*cloud_)[cp].r;
502 colors[idx + 1] = (*cloud_)[cp].g;
503 colors[idx + 2] = (*cloud_)[cp].b;
504 colors[idx + 3] = (*cloud_)[cp].a;
505 }
506 }
507 return scalars;
508}
509
510
511template <typename PointT> void
513{
515 field_idx_ = pcl::getFieldIndex<PointT> ("label", fields_);
516 if (field_idx_ != -1)
517 {
518 capable_ = true;
519 return;
520 }
521}
522
523
524template <typename PointT> vtkSmartPointer<vtkDataArray>
526{
527 if (!capable_ || !cloud_)
528 return nullptr;
529
531 scalars->SetNumberOfComponents (3);
532
533 vtkIdType nr_points = cloud_->size ();
534 scalars->SetNumberOfTuples (nr_points);
535 unsigned char* colors = scalars->GetPointer (0);
536
537
538 std::map<std::uint32_t, pcl::RGB> colormap;
539 if (!static_mapping_)
540 {
541 std::set<std::uint32_t> labels;
542 // First pass: find unique labels
543 for (vtkIdType i = 0; i < nr_points; ++i)
544 labels.insert ((*cloud_)[i].label);
545
546 // Assign Glasbey colors in ascending order of labels
547 std::size_t color = 0;
548 for (std::set<std::uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
549 colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
550 }
551
552 int j = 0;
553 for (vtkIdType cp = 0; cp < nr_points; ++cp)
554 {
555 if (pcl::isFinite ((*cloud_)[cp]))
556 {
557 const pcl::RGB& color = static_mapping_ ? GlasbeyLUT::at ((*cloud_)[cp].label % GlasbeyLUT::size ()) : colormap[(*cloud_)[cp].label];
558 colors[j ] = color.r;
559 colors[j + 1] = color.g;
560 colors[j + 2] = color.b;
561 j += 3;
562 }
563 }
564
565 return scalars;
566}
567
568} // namespace visualization
569} // namespace pcl
570
static std::size_t size()
Get the number of colors in the lookup table.
static RGB at(std::size_t color_id)
Get a color from the lookup table with a given id.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
PointCloudColorHandlerHSVField(const PointCloudConstPtr &cloud)
Constructor.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
Base Handler class for PointCloud colors.
bool capable_
True if this handler is capable of handling the input data, false otherwise.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
std::vector< pcl::PCLPointField > fields_
The list of fields available for this PointCloud.
int field_idx_
The index of the field holding the data that represents the color.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
vtkSmartPointer< vtkDataArray > getColor() const override
Obtain the actual color for the input dataset as a VTK data array.
int getFieldSize(const int datatype)
Obtains the size of a specific field data type in bytes.
Definition io.h:119
PCL_EXPORTS void getRandomColors(double &r, double &g, double &b, double min=0.2, double max=2.8)
Get (good) random values for R/G/B.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
Defines all the PCL and non-PCL macros used.
#define pcl_lrint(x)
Definition pcl_macros.h:253
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.