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