Point Cloud Library (PCL)  1.14.1-dev
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 
49 namespace pcl
50 {
51 
52 namespace visualization
53 {
54 
55 template <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 
82 template <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 
115 template <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 
138 template <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  // If XYZ present, check if the points are invalid
161  int x_idx = -1;
162  for (std::size_t d = 0; d < fields_.size (); ++d)
163  if (fields_[d].name == "x")
164  x_idx = static_cast<int> (d);
165 
166  pcl::RGB rgb;
167  if (x_idx != -1)
168  {
169  int j = 0;
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  memcpy (&rgb, (reinterpret_cast<const char *> (&(*cloud_)[cp])) + rgba_offset, sizeof (pcl::RGB));
179  colors[j ] = rgb.r;
180  colors[j + 1] = rgb.g;
181  colors[j + 2] = rgb.b;
182  j += 3;
183  }
184  }
185  else
186  {
187  // Color every point
188  for (vtkIdType cp = 0; cp < nr_points; ++cp)
189  {
190  int idx = static_cast<int> (cp) * 3;
191  memcpy (&rgb, (reinterpret_cast<const char *> (&(*cloud_)[cp])) + rgba_offset, sizeof (pcl::RGB));
192  colors[idx ] = rgb.r;
193  colors[idx + 1] = rgb.g;
194  colors[idx + 2] = rgb.b;
195  }
196  }
197  return scalars;
198 }
199 
200 
201 template <typename PointT>
204 {
205  // Check for the presence of the "H" field
206  field_idx_ = pcl::getFieldIndex<PointT> ("h", fields_);
207  if (field_idx_ == -1)
208  {
209  capable_ = false;
210  return;
211  }
212 
213  // Check for the presence of the "S" field
214  s_field_idx_ = pcl::getFieldIndex<PointT> ("s", fields_);
215  if (s_field_idx_ == -1)
216  {
217  capable_ = false;
218  return;
219  }
220 
221  // Check for the presence of the "V" field
222  v_field_idx_ = pcl::getFieldIndex<PointT> ("v", fields_);
223  if (v_field_idx_ == -1)
224  {
225  capable_ = false;
226  return;
227  }
228  capable_ = true;
229 }
230 
231 
232 template <typename PointT> vtkSmartPointer<vtkDataArray>
234 {
235  if (!capable_ || !cloud_)
236  return nullptr;
237 
239  scalars->SetNumberOfComponents (3);
240 
241  vtkIdType nr_points = cloud_->size ();
242  scalars->SetNumberOfTuples (nr_points);
243  unsigned char* colors = scalars->GetPointer (0);
244 
245  int idx = 0;
246  // If XYZ present, check if the points are invalid
247  int x_idx = -1;
248 
249  for (std::size_t d = 0; d < fields_.size (); ++d)
250  if (fields_[d].name == "x")
251  x_idx = static_cast<int> (d);
252 
253  if (x_idx != -1)
254  {
255  // Color every point
256  for (vtkIdType cp = 0; cp < nr_points; ++cp)
257  {
258  // Copy the value at the specified field
259  if (!std::isfinite ((*cloud_)[cp].x) ||
260  !std::isfinite ((*cloud_)[cp].y) ||
261  !std::isfinite ((*cloud_)[cp].z))
262  continue;
263 
264  ///@todo do this with the point_types_conversion in common, first template it!
265 
266  float h = (*cloud_)[cp].h;
267  float v = (*cloud_)[cp].v;
268  float s = (*cloud_)[cp].s;
269 
270  // Fill color data with HSV here:
271  // restrict the hue value to [0,360[
272  h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
273 
274  // restrict s and v to [0,1]
275  if (s > 1.0f) s = 1.0f;
276  if (s < 0.0f) s = 0.0f;
277  if (v > 1.0f) v = 1.0f;
278  if (v < 0.0f) v = 0.0f;
279 
280  if (s == 0.0f)
281  {
282  colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
283  }
284  else
285  {
286  // calculate p, q, t from HSV-values
287  float a = h / 60;
288  int i = std::floor (a);
289  float f = a - i;
290  float p = v * (1 - s);
291  float q = v * (1 - s * f);
292  float t = v * (1 - s * (1 - f));
293 
294  switch (i)
295  {
296  case 0:
297  colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
298  case 1:
299  colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
300  case 2:
301  colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
302  case 3:
303  colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
304  case 4:
305  colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
306  case 5:
307  colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
308  }
309  }
310  idx +=3;
311  }
312  }
313  else
314  {
315  // Color every point
316  for (vtkIdType cp = 0; cp < nr_points; ++cp)
317  {
318  float h = (*cloud_)[cp].h;
319  float v = (*cloud_)[cp].v;
320  float s = (*cloud_)[cp].s;
321 
322  // Fill color data with HSV here:
323  // restrict the hue value to [0,360[
324  h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
325 
326  // restrict s and v to [0,1]
327  if (s > 1.0f) s = 1.0f;
328  if (s < 0.0f) s = 0.0f;
329  if (v > 1.0f) v = 1.0f;
330  if (v < 0.0f) v = 0.0f;
331 
332  if (s == 0.0f)
333  {
334  colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
335  }
336  else
337  {
338  // calculate p, q, t from HSV-values
339  float a = h / 60;
340  int i = std::floor (a);
341  float f = a - i;
342  float p = v * (1 - s);
343  float q = v * (1 - s * f);
344  float t = v * (1 - s * (1 - f));
345 
346  switch (i)
347  {
348  case 0:
349  colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
350  case 1:
351  colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
352  case 2:
353  colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
354  case 3:
355  colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
356  case 4:
357  colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
358  case 5:
359  colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
360  }
361  }
362  idx +=3;
363  }
364  }
365  return scalars;
366 }
367 
368 
369 template <typename PointT> void
371  const PointCloudConstPtr &cloud)
372 {
374  field_idx_ = pcl::getFieldIndex<PointT> (field_name_, fields_);
375  if (field_idx_ != -1)
376  capable_ = true;
377  else
378  capable_ = false;
379 }
380 
381 
382 template <typename PointT> vtkSmartPointer<vtkDataArray>
384 {
385  if (!capable_ || !cloud_)
386  return nullptr;
387 
388  auto scalars = vtkSmartPointer<vtkFloatArray>::New ();
389  scalars->SetNumberOfComponents (1);
390 
391  vtkIdType nr_points = cloud_->size ();
392  scalars->SetNumberOfTuples (nr_points);
393 
394  using FieldList = typename pcl::traits::fieldList<PointT>::type;
395 
396  float* colors = new float[nr_points];
397  float field_data;
398 
399  int j = 0;
400  // If XYZ present, check if the points are invalid
401  int x_idx = -1;
402  for (std::size_t d = 0; d < fields_.size (); ++d)
403  if (fields_[d].name == "x")
404  x_idx = static_cast<int> (d);
405 
406  if (x_idx != -1)
407  {
408  // Color every point
409  for (vtkIdType cp = 0; cp < nr_points; ++cp)
410  {
411  // Copy the value at the specified field
412  if (!std::isfinite ((*cloud_)[cp].x) || !std::isfinite ((*cloud_)[cp].y) || !std::isfinite ((*cloud_)[cp].z))
413  continue;
414 
415  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud_)[cp]);
416  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
417 
418  colors[j] = field_data;
419  j++;
420  }
421  }
422  else
423  {
424  // Color every point
425  for (vtkIdType cp = 0; cp < nr_points; ++cp)
426  {
427  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud_)[cp]);
428  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
429 
430  if (!std::isfinite (field_data))
431  continue;
432 
433  colors[j] = field_data;
434  j++;
435  }
436  }
437  scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
438  return scalars;
439 }
440 
441 
442 template <typename PointT> void
444  const PointCloudConstPtr &cloud)
445 {
447  // Handle the 24-bit packed RGBA values
448  field_idx_ = pcl::getFieldIndex<PointT> ("rgba", fields_);
449  if (field_idx_ != -1)
450  capable_ = true;
451  else
452  capable_ = false;
453 }
454 
455 
456 template <typename PointT> vtkSmartPointer<vtkDataArray>
458 {
459  if (!capable_ || !cloud_)
460  return nullptr;
461 
463  scalars->SetNumberOfComponents (4);
464 
465  vtkIdType nr_points = cloud_->size ();
466  scalars->SetNumberOfTuples (nr_points);
467  unsigned char* colors = scalars->GetPointer (0);
468 
469  // If XYZ present, check if the points are invalid
470  int x_idx = -1;
471  for (std::size_t d = 0; d < fields_.size (); ++d)
472  if (fields_[d].name == "x")
473  x_idx = static_cast<int> (d);
474 
475  if (x_idx != -1)
476  {
477  int j = 0;
478  // Color every point
479  for (vtkIdType cp = 0; cp < nr_points; ++cp)
480  {
481  // Copy the value at the specified field
482  if (!std::isfinite ((*cloud_)[cp].x) ||
483  !std::isfinite ((*cloud_)[cp].y) ||
484  !std::isfinite ((*cloud_)[cp].z))
485  continue;
486 
487  colors[j ] = (*cloud_)[cp].r;
488  colors[j + 1] = (*cloud_)[cp].g;
489  colors[j + 2] = (*cloud_)[cp].b;
490  colors[j + 3] = (*cloud_)[cp].a;
491  j += 4;
492  }
493  }
494  else
495  {
496  // Color every point
497  for (vtkIdType cp = 0; cp < nr_points; ++cp)
498  {
499  int idx = static_cast<int> (cp) * 4;
500  colors[idx ] = (*cloud_)[cp].r;
501  colors[idx + 1] = (*cloud_)[cp].g;
502  colors[idx + 2] = (*cloud_)[cp].b;
503  colors[idx + 3] = (*cloud_)[cp].a;
504  }
505  }
506  return scalars;
507 }
508 
509 
510 template <typename PointT> void
512 {
514  field_idx_ = pcl::getFieldIndex<PointT> ("label", fields_);
515  if (field_idx_ != -1)
516  {
517  capable_ = true;
518  return;
519  }
520 }
521 
522 
523 template <typename PointT> vtkSmartPointer<vtkDataArray>
525 {
526  if (!capable_ || !cloud_)
527  return nullptr;
528 
530  scalars->SetNumberOfComponents (3);
531 
532  vtkIdType nr_points = cloud_->size ();
533  scalars->SetNumberOfTuples (nr_points);
534  unsigned char* colors = scalars->GetPointer (0);
535 
536 
537  std::map<std::uint32_t, pcl::RGB> colormap;
538  if (!static_mapping_)
539  {
540  std::set<std::uint32_t> labels;
541  // First pass: find unique labels
542  for (vtkIdType i = 0; i < nr_points; ++i)
543  labels.insert ((*cloud_)[i].label);
544 
545  // Assign Glasbey colors in ascending order of labels
546  std::size_t color = 0;
547  for (std::set<std::uint32_t>::iterator iter = labels.begin (); iter != labels.end (); ++iter, ++color)
548  colormap[*iter] = GlasbeyLUT::at (color % GlasbeyLUT::size ());
549  }
550 
551  int j = 0;
552  for (vtkIdType cp = 0; cp < nr_points; ++cp)
553  {
554  if (pcl::isFinite ((*cloud_)[cp]))
555  {
556  const pcl::RGB& color = static_mapping_ ? GlasbeyLUT::at ((*cloud_)[cp].label % GlasbeyLUT::size ()) : colormap[(*cloud_)[cp].label];
557  colors[j ] = color.r;
558  colors[j + 1] = color.g;
559  colors[j + 2] = color.b;
560  j += 3;
561  }
562  }
563 
564  return scalars;
565 }
566 
567 } // namespace visualization
568 } // namespace pcl
569 
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:127
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.