Point Cloud Library (PCL)  1.7.2
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 <pcl/pcl_macros.h>
42 
43 ///////////////////////////////////////////////////////////////////////////////////////////
44 template <typename PointT> bool
46 {
47  if (!capable_ || !cloud_)
48  return (false);
49 
50  if (!scalars)
52  scalars->SetNumberOfComponents (3);
53 
54  vtkIdType nr_points = cloud_->points.size ();
55  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
56 
57  // Get a random color
58  unsigned char* colors = new unsigned char[nr_points * 3];
59 
60  // Color every point
61  for (vtkIdType cp = 0; cp < nr_points; ++cp)
62  {
63  colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
64  colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
65  colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
66  }
67  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
68  return (true);
69 }
70 
71 ///////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT> bool
74 {
75  if (!capable_ || !cloud_)
76  return (false);
77 
78  if (!scalars)
80  scalars->SetNumberOfComponents (3);
81 
82  vtkIdType nr_points = cloud_->points.size ();
83  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
84 
85  // Get a random color
86  unsigned char* colors = new unsigned char[nr_points * 3];
87  double r, g, b;
89 
90  int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
91  g_ = static_cast<int> (pcl_lrint (g * 255.0)),
92  b_ = static_cast<int> (pcl_lrint (b * 255.0));
93 
94  // Color every point
95  for (vtkIdType cp = 0; cp < nr_points; ++cp)
96  {
97  colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
98  colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
99  colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
100  }
101  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
102  return (true);
103 }
104 
105 ///////////////////////////////////////////////////////////////////////////////////////////
106 template <typename PointT> void
108  const PointCloudConstPtr &cloud)
109 {
111  // Handle the 24-bit packed RGB values
112  field_idx_ = pcl::getFieldIndex (*cloud, "rgb", fields_);
113  if (field_idx_ != -1)
114  {
115  capable_ = true;
116  return;
117  }
118  else
119  {
120  field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_);
121  if (field_idx_ != -1)
122  capable_ = true;
123  else
124  capable_ = false;
125  }
126 }
127 
128 ///////////////////////////////////////////////////////////////////////////////////////////
129 template <typename PointT> bool
131 {
132  if (!capable_ || !cloud_)
133  return (false);
134 
135  if (!scalars)
137  scalars->SetNumberOfComponents (3);
138 
139  vtkIdType nr_points = cloud_->points.size ();
140  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
141  unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
142 
143  int j = 0;
144  // If XYZ present, check if the points are invalid
145  int x_idx = -1;
146  for (size_t d = 0; d < fields_.size (); ++d)
147  if (fields_[d].name == "x")
148  x_idx = static_cast<int> (d);
149 
150  if (x_idx != -1)
151  {
152  // Color every point
153  for (vtkIdType cp = 0; cp < nr_points; ++cp)
154  {
155  // Copy the value at the specified field
156  if (!pcl_isfinite (cloud_->points[cp].x) ||
157  !pcl_isfinite (cloud_->points[cp].y) ||
158  !pcl_isfinite (cloud_->points[cp].z))
159  continue;
160 
161  colors[j ] = cloud_->points[cp].r;
162  colors[j + 1] = cloud_->points[cp].g;
163  colors[j + 2] = cloud_->points[cp].b;
164  j += 3;
165  }
166  }
167  else
168  {
169  // Color every point
170  for (vtkIdType cp = 0; cp < nr_points; ++cp)
171  {
172  int idx = static_cast<int> (cp) * 3;
173  colors[idx ] = cloud_->points[cp].r;
174  colors[idx + 1] = cloud_->points[cp].g;
175  colors[idx + 2] = cloud_->points[cp].b;
176  }
177  }
178  return (true);
179 }
180 
181 ///////////////////////////////////////////////////////////////////////////////////////////
182 template <typename PointT>
184  pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
185 {
186  // Check for the presence of the "H" field
187  field_idx_ = pcl::getFieldIndex (*cloud, "h", fields_);
188  if (field_idx_ == -1)
189  {
190  capable_ = false;
191  return;
192  }
193 
194  // Check for the presence of the "S" field
195  s_field_idx_ = pcl::getFieldIndex (*cloud, "s", fields_);
196  if (s_field_idx_ == -1)
197  {
198  capable_ = false;
199  return;
200  }
201 
202  // Check for the presence of the "V" field
203  v_field_idx_ = pcl::getFieldIndex (*cloud, "v", fields_);
204  if (v_field_idx_ == -1)
205  {
206  capable_ = false;
207  return;
208  }
209  capable_ = true;
210 }
211 
212 ///////////////////////////////////////////////////////////////////////////////////////////
213 template <typename PointT> bool
215 {
216  if (!capable_ || !cloud_)
217  return (false);
218 
219  if (!scalars)
221  scalars->SetNumberOfComponents (3);
222 
223  vtkIdType nr_points = cloud_->points.size ();
224  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
225  unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
226 
227  int idx = 0;
228  // If XYZ present, check if the points are invalid
229  int x_idx = -1;
230 
231  for (size_t d = 0; d < fields_.size (); ++d)
232  if (fields_[d].name == "x")
233  x_idx = static_cast<int> (d);
234 
235  if (x_idx != -1)
236  {
237  // Color every point
238  for (vtkIdType cp = 0; cp < nr_points; ++cp)
239  {
240  // Copy the value at the specified field
241  if (!pcl_isfinite (cloud_->points[cp].x) ||
242  !pcl_isfinite (cloud_->points[cp].y) ||
243  !pcl_isfinite (cloud_->points[cp].z))
244  continue;
245 
246  ///@todo do this with the point_types_conversion in common, first template it!
247 
248  float h = cloud_->points[cp].h;
249  float v = cloud_->points[cp].v;
250  float s = cloud_->points[cp].s;
251 
252  // Fill color data with HSV here:
253  // restrict the hue value to [0,360[
254  h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
255 
256  // restrict s and v to [0,1]
257  if (s > 1.0f) s = 1.0f;
258  if (s < 0.0f) s = 0.0f;
259  if (v > 1.0f) v = 1.0f;
260  if (v < 0.0f) v = 0.0f;
261 
262  if (s == 0.0f)
263  {
264  colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
265  }
266  else
267  {
268  // calculate p, q, t from HSV-values
269  float a = h / 60;
270  int i = floor (a);
271  float f = a - i;
272  float p = v * (1 - s);
273  float q = v * (1 - s * f);
274  float t = v * (1 - s * (1 - f));
275 
276  switch (i)
277  {
278  case 0:
279  colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
280  case 1:
281  colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
282  case 2:
283  colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
284  case 3:
285  colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
286  case 4:
287  colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
288  case 5:
289  colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
290  }
291  }
292  idx +=3;
293  }
294  }
295  else
296  {
297  // Color every point
298  for (vtkIdType cp = 0; cp < nr_points; ++cp)
299  {
300  float h = cloud_->points[cp].h;
301  float v = cloud_->points[cp].v;
302  float s = cloud_->points[cp].s;
303 
304  // Fill color data with HSV here:
305  // restrict the hue value to [0,360[
306  h = h < 0.0f ? h - (((int)h)/360 - 1)*360 : h - (((int)h)/360)*360;
307 
308  // restrict s and v to [0,1]
309  if (s > 1.0f) s = 1.0f;
310  if (s < 0.0f) s = 0.0f;
311  if (v > 1.0f) v = 1.0f;
312  if (v < 0.0f) v = 0.0f;
313 
314  if (s == 0.0f)
315  {
316  colors[idx] = colors[idx+1] = colors[idx+2] = v*255;
317  }
318  else
319  {
320  // calculate p, q, t from HSV-values
321  float a = h / 60;
322  int i = floor (a);
323  float f = a - i;
324  float p = v * (1 - s);
325  float q = v * (1 - s * f);
326  float t = v * (1 - s * (1 - f));
327 
328  switch (i)
329  {
330  case 0:
331  colors[idx] = v*255; colors[idx+1] = t*255; colors[idx+2] = p*255; break;
332  case 1:
333  colors[idx] = q*255; colors[idx+1] = v*255; colors[idx+2] = p*255; break;
334  case 2:
335  colors[idx] = p*255; colors[idx+1] = v*255; colors[idx+2] = t*255; break;
336  case 3:
337  colors[idx] = p*255; colors[idx+1] = q*255; colors[idx+2] = v*255; break;
338  case 4:
339  colors[idx] = t*255; colors[idx+1] = p*255; colors[idx+2] = v*255; break;
340  case 5:
341  colors[idx] = v*255; colors[idx+1] = p*255; colors[idx+2] = q*255; break;
342  }
343  }
344  idx +=3;
345  }
346  }
347  return (true);
348 }
349 
350 ///////////////////////////////////////////////////////////////////////////////////////////
351 template <typename PointT> void
353  const PointCloudConstPtr &cloud)
354 {
356  field_idx_ = pcl::getFieldIndex (*cloud, field_name_, fields_);
357  if (field_idx_ != -1)
358  capable_ = true;
359  else
360  capable_ = false;
361 }
362 
363 ///////////////////////////////////////////////////////////////////////////////////////////
364 template <typename PointT> bool
366 {
367  if (!capable_ || !cloud_)
368  return (false);
369 
370  if (!scalars)
372  scalars->SetNumberOfComponents (1);
373 
374  vtkIdType nr_points = cloud_->points.size ();
375  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
376 
377  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
378 
379  float* colors = new float[nr_points];
380  float field_data;
381 
382  int j = 0;
383  // If XYZ present, check if the points are invalid
384  int x_idx = -1;
385  for (size_t d = 0; d < fields_.size (); ++d)
386  if (fields_[d].name == "x")
387  x_idx = static_cast<int> (d);
388 
389  if (x_idx != -1)
390  {
391  // Color every point
392  for (vtkIdType cp = 0; cp < nr_points; ++cp)
393  {
394  // Copy the value at the specified field
395  if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z))
396  continue;
397 
398  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
399  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
400 
401  colors[j] = field_data;
402  j++;
403  }
404  }
405  else
406  {
407  // Color every point
408  for (vtkIdType cp = 0; cp < nr_points; ++cp)
409  {
410  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
411  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
412 
413  if (!pcl_isfinite (field_data))
414  continue;
415 
416  colors[j] = field_data;
417  j++;
418  }
419  }
420  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
421  return (true);
422 }
423 
424 ///////////////////////////////////////////////////////////////////////////////////////////
425 template <typename PointT> void
427  const PointCloudConstPtr &cloud)
428 {
430  // Handle the 24-bit packed RGBA values
431  field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_);
432  if (field_idx_ != -1)
433  capable_ = true;
434  else
435  capable_ = false;
436 }
437 
438 ///////////////////////////////////////////////////////////////////////////////////////////
439 template <typename PointT> bool
441 {
442  if (!capable_ || !cloud_)
443  return (false);
444 
445  if (!scalars)
447  scalars->SetNumberOfComponents (4);
448 
449  vtkIdType nr_points = cloud_->points.size ();
450  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
451  unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
452 
453  int j = 0;
454  // If XYZ present, check if the points are invalid
455  int x_idx = -1;
456  for (size_t d = 0; d < fields_.size (); ++d)
457  if (fields_[d].name == "x")
458  x_idx = static_cast<int> (d);
459 
460  if (x_idx != -1)
461  {
462  // Color every point
463  for (vtkIdType cp = 0; cp < nr_points; ++cp)
464  {
465  // Copy the value at the specified field
466  if (!pcl_isfinite (cloud_->points[cp].x) ||
467  !pcl_isfinite (cloud_->points[cp].y) ||
468  !pcl_isfinite (cloud_->points[cp].z))
469  continue;
470 
471  colors[j ] = cloud_->points[cp].r;
472  colors[j + 1] = cloud_->points[cp].g;
473  colors[j + 2] = cloud_->points[cp].b;
474  colors[j + 3] = cloud_->points[cp].a;
475  j += 4;
476  }
477  }
478  else
479  {
480  // Color every point
481  for (vtkIdType cp = 0; cp < nr_points; ++cp)
482  {
483  int idx = static_cast<int> (cp) * 4;
484  colors[idx ] = cloud_->points[cp].r;
485  colors[idx + 1] = cloud_->points[cp].g;
486  colors[idx + 2] = cloud_->points[cp].b;
487  colors[idx + 3] = cloud_->points[cp].a;
488  }
489  }
490  return (true);
491 }
492 
493 #endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
494 
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 void setInputCloud(const PointCloudConstPtr &cloud)
Set the input cloud to be used.
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.
PointCloudColorHandlerHSVField(const PointCloudConstPtr &cloud)
Constructor.
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const
Obtain the actual color for the input dataset as vtk scalars.
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.
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
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const
Obtain the actual color for the input dataset as vtk scalars.
PointCloudConstPtr cloud_
A pointer to the input dataset.
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.
bool capable_
True if this handler is capable of handling the input data, false otherwise.
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const
Obtain the actual color for the input dataset as vtk scalars.