Point Cloud Library (PCL)  1.7.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 <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 j = 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  int idx = j * 3;
247 
248  ///@todo do this with the point_types_conversion in common, first template it!
249 
250  // Fill color data with HSV here:
251  if (cloud_->points[cp].s == 0)
252  {
253  colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
254  return;
255  }
256  float a = cloud_->points[cp].h / 60;
257  int i = floor (a);
258  float f = a - i;
259  float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
260  float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
261  float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
262 
263  switch (i)
264  {
265  case 0:
266  colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
267  case 1:
268  colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
269  case 2:
270  colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
271  case 3:
272  colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
273  case 4:
274  colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
275  default:
276  colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
277  }
278  j++;
279  }
280  }
281  else
282  {
283  // Color every point
284  for (vtkIdType cp = 0; cp < nr_points; ++cp)
285  {
286  int idx = cp * 3;
287 
288  // Fill color data with HSV here:
289  if (cloud_->points[cp].s == 0)
290  {
291  colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
292  return;
293  }
294  float a = cloud_->points[cp].h / 60;
295  int i = floor (a);
296  float f = a - i;
297  float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
298  float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
299  float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
300 
301  switch (i)
302  {
303  case 0:
304  colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
305  case 1:
306  colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
307  case 2:
308  colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
309  case 3:
310  colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
311  case 4:
312  colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
313  default:
314  colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
315  }
316  }
317  }
318  return (true);
319 }
320 
321 ///////////////////////////////////////////////////////////////////////////////////////////
322 template <typename PointT> void
324  const PointCloudConstPtr &cloud)
325 {
327  field_idx_ = pcl::getFieldIndex (*cloud, field_name_, fields_);
328  if (field_idx_ != -1)
329  capable_ = true;
330  else
331  capable_ = false;
332 }
333 
334 ///////////////////////////////////////////////////////////////////////////////////////////
335 template <typename PointT> bool
337 {
338  if (!capable_ || !cloud_)
339  return (false);
340 
341  if (!scalars)
343  scalars->SetNumberOfComponents (1);
344 
345  vtkIdType nr_points = cloud_->points.size ();
346  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
347 
348  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
349 
350  float* colors = new float[nr_points];
351  float field_data;
352 
353  int j = 0;
354  // If XYZ present, check if the points are invalid
355  int x_idx = -1;
356  for (size_t d = 0; d < fields_.size (); ++d)
357  if (fields_[d].name == "x")
358  x_idx = static_cast<int> (d);
359 
360  if (x_idx != -1)
361  {
362  // Color every point
363  for (vtkIdType cp = 0; cp < nr_points; ++cp)
364  {
365  // Copy the value at the specified field
366  if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z))
367  continue;
368 
369  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
370  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
371 
372  colors[j] = field_data;
373  j++;
374  }
375  }
376  else
377  {
378  // Color every point
379  for (vtkIdType cp = 0; cp < nr_points; ++cp)
380  {
381  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
382  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
383 
384  if (!pcl_isfinite (field_data))
385  continue;
386 
387  colors[j] = field_data;
388  j++;
389  }
390  }
391  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
392  return (true);
393 }
394 
395 #endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
396