Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
point_cloud_handlers.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: point_cloud_handlers.hpp 6161 2012-07-05 17:37:29Z rusu $
35  *
36  */
37 
38 #include <pcl/console/time.h>
39 #include <pcl/pcl_macros.h>
40 
42 template <typename PointT> void
43 pcl::visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
44 {
45  if (!capable_)
46  return;
47 
48  if (!scalars)
49  scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
50  scalars->SetNumberOfComponents (3);
51 
52  vtkIdType nr_points = cloud_->points.size ();
53  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
54 
55  // Get a random color
56  unsigned char* colors = new unsigned char[nr_points * 3];
57 
58  // Color every point
59  for (vtkIdType cp = 0; cp < nr_points; ++cp)
60  {
61  colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
62  colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
63  colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
64  }
65  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
66 }
67 
69 template <typename PointT> void
70 pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
71 {
72  if (!capable_)
73  return;
74 
75  if (!scalars)
76  scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
77  scalars->SetNumberOfComponents (3);
78 
79  vtkIdType nr_points = cloud_->points.size ();
80  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
81 
82  // Get a random color
83  unsigned char* colors = new unsigned char[nr_points * 3];
84  double r, g, b;
85  pcl::visualization::getRandomColors (r, g, b);
86 
87  int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
88  g_ = static_cast<int> (pcl_lrint (g * 255.0)),
89  b_ = static_cast<int> (pcl_lrint (b * 255.0));
90 
91  // Color every point
92  for (vtkIdType cp = 0; cp < nr_points; ++cp)
93  {
94  colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
95  colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
96  colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
97  }
98  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
99 }
100 
102 template <typename PointT>
103 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::PointCloudColorHandlerRGBField (const PointCloudConstPtr &cloud) :
104  pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
105 {
106  // Handle the 24-bit packed RGB values
107  field_idx_ = pcl::getFieldIndex (*cloud, "rgb", fields_);
108  if (field_idx_ != -1)
109  {
110  capable_ = true;
111  return;
112  }
113  else
114  {
115  field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_);
116  if (field_idx_ != -1)
117  capable_ = true;
118  else
119  capable_ = false;
120  }
121 }
122 
124 template <typename PointT> void
125 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
126 {
127  if (!capable_)
128  return;
129 
130  if (!scalars)
131  scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
132  scalars->SetNumberOfComponents (3);
133 
134  vtkIdType nr_points = cloud_->points.size ();
135  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
136  unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
137 
138  int j = 0;
139  // If XYZ present, check if the points are invalid
140  int x_idx = -1;
141  for (size_t d = 0; d < fields_.size (); ++d)
142  if (fields_[d].name == "x")
143  x_idx = static_cast<int> (d);
144 
145  if (x_idx != -1)
146  {
147  // Color every point
148  for (vtkIdType cp = 0; cp < nr_points; ++cp)
149  {
150  // Copy the value at the specified field
151  if (!pcl_isfinite (cloud_->points[cp].x) ||
152  !pcl_isfinite (cloud_->points[cp].y) ||
153  !pcl_isfinite (cloud_->points[cp].z))
154  continue;
155 
156  int idx = j * 3;
157  colors[idx ] = cloud_->points[cp].r;
158  colors[idx + 1] = cloud_->points[cp].g;
159  colors[idx + 2] = cloud_->points[cp].b;
160  j++;
161  }
162  }
163  else
164  {
165  // Color every point
166  for (vtkIdType cp = 0; cp < nr_points; ++cp)
167  {
168  int idx = static_cast<int> (cp) * 3;
169  colors[idx ] = cloud_->points[cp].r;
170  colors[idx + 1] = cloud_->points[cp].g;
171  colors[idx + 2] = cloud_->points[cp].b;
172  }
173  }
174 }
175 
177 template <typename PointT>
178 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) :
179  pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
180 {
181  // Check for the presence of the "H" field
182  field_idx_ = pcl::getFieldIndex (*cloud, "h", fields_);
183  if (field_idx_ == -1)
184  {
185  capable_ = false;
186  return;
187  }
188 
189  // Check for the presence of the "S" field
190  s_field_idx_ = pcl::getFieldIndex (*cloud, "s", fields_);
191  if (s_field_idx_ == -1)
192  {
193  capable_ = false;
194  return;
195  }
196 
197  // Check for the presence of the "V" field
198  v_field_idx_ = pcl::getFieldIndex (*cloud, "v", fields_);
199  if (v_field_idx_ == -1)
200  {
201  capable_ = false;
202  return;
203  }
204  capable_ = true;
205 }
206 
208 template <typename PointT> void
209 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
210 {
211  if (!capable_)
212  return;
213 
214  if (!scalars)
215  scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
216  scalars->SetNumberOfComponents (3);
217 
218  vtkIdType nr_points = cloud_->points.size ();
219  reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
220  unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
221 
222  int j = 0;
223  // If XYZ present, check if the points are invalid
224  int x_idx = -1;
225 
226  for (size_t d = 0; d < fields_.size (); ++d)
227  if (fields_[d].name == "x")
228  x_idx = static_cast<int> (d);
229 
230  if (x_idx != -1)
231  {
232  // Color every point
233  for (vtkIdType cp = 0; cp < nr_points; ++cp)
234  {
235  // Copy the value at the specified field
236  if (!pcl_isfinite (cloud_->points[cp].x) ||
237  !pcl_isfinite (cloud_->points[cp].y) ||
238  !pcl_isfinite (cloud_->points[cp].z))
239  continue;
240 
241  int idx = j * 3;
242 
244 
245  // Fill color data with HSV here:
246  if (cloud_->points[cp].s == 0)
247  {
248  colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
249  return;
250  }
251  float a = cloud_->points[cp].h / 60;
252  int i = floor (a);
253  float f = a - i;
254  float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
255  float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
256  float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
257 
258  switch (i)
259  {
260  case 0:
261  colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
262  case 1:
263  colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
264  case 2:
265  colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
266  case 3:
267  colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
268  case 4:
269  colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
270  default:
271  colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
272  }
273  j++;
274  }
275  }
276  else
277  {
278  // Color every point
279  for (vtkIdType cp = 0; cp < nr_points; ++cp)
280  {
281  int idx = cp * 3;
282 
283  // Fill color data with HSV here:
284  if (cloud_->points[cp].s == 0)
285  {
286  colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
287  return;
288  }
289  float a = cloud_->points[cp].h / 60;
290  int i = floor (a);
291  float f = a - i;
292  float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
293  float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
294  float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
295 
296  switch (i)
297  {
298  case 0:
299  colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
300  case 1:
301  colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
302  case 2:
303  colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
304  case 3:
305  colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
306  case 4:
307  colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
308  default:
309  colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
310  }
311  }
312  }
313 }
314 
316 template <typename PointT>
317 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::PointCloudColorHandlerGenericField (
318  const PointCloudConstPtr &cloud, const std::string &field_name) :
319  pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud),
320  field_name_ (field_name)
321 {
322  field_idx_ = pcl::getFieldIndex (*cloud, field_name, fields_);
323  if (field_idx_ != -1)
324  capable_ = true;
325  else
326  capable_ = false;
327 }
328 
330 template <typename PointT> void
331 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
332 {
333  if (!capable_)
334  return;
335 
336  if (!scalars)
337  scalars = vtkSmartPointer<vtkFloatArray>::New ();
338  scalars->SetNumberOfComponents (1);
339 
340  vtkIdType nr_points = cloud_->points.size ();
341  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
342 
343  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
344 
345  float* colors = new float[nr_points];
346  float field_data;
347 
348  int j = 0;
349  // If XYZ present, check if the points are invalid
350  int x_idx = -1;
351  for (size_t d = 0; d < fields_.size (); ++d)
352  if (fields_[d].name == "x")
353  x_idx = static_cast<int> (d);
354 
355  if (x_idx != -1)
356  {
357  // Color every point
358  for (vtkIdType cp = 0; cp < nr_points; ++cp)
359  {
360  // Copy the value at the specified field
361  if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z))
362  continue;
363 
364  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
365  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
366 
367  colors[j] = field_data;
368  j++;
369  }
370  }
371  else
372  {
373  // Color every point
374  for (vtkIdType cp = 0; cp < nr_points; ++cp)
375  {
376  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
377  memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
378 
379  if (!pcl_isfinite (field_data))
380  continue;
381 
382  colors[j] = field_data;
383  j++;
384  }
385  }
386  reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
387 }
388 
390 template <typename PointT>
391 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const PointCloudConstPtr &cloud)
392  : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
393 {
394  field_x_idx_ = pcl::getFieldIndex (*cloud, "x", fields_);
395  if (field_x_idx_ == -1)
396  return;
397  field_y_idx_ = pcl::getFieldIndex (*cloud, "y", fields_);
398  if (field_y_idx_ == -1)
399  return;
400  field_z_idx_ = pcl::getFieldIndex (*cloud, "z", fields_);
401  if (field_z_idx_ == -1)
402  return;
403  capable_ = true;
404 }
405 
407 template <typename PointT> void
408 pcl::visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
409 {
410  if (!capable_)
411  return;
412 
413  if (!points)
414  points = vtkSmartPointer<vtkPoints>::New ();
415  points->SetDataTypeToFloat ();
416 
417  vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
418  data->SetNumberOfComponents (3);
419  vtkIdType nr_points = cloud_->points.size ();
420 
421  // Add all points
422  vtkIdType j = 0; // true point index
423  float* pts = static_cast<float*> (malloc (nr_points * 3 * sizeof (float)));
424 
425  // If the dataset has no invalid values, just copy all of them
426  if (cloud_->is_dense)
427  {
428  for (vtkIdType i = 0; i < nr_points; ++i)
429  {
430  pts[i * 3 + 0] = cloud_->points[i].x;
431  pts[i * 3 + 1] = cloud_->points[i].y;
432  pts[i * 3 + 2] = cloud_->points[i].z;
433  }
434  data->SetArray (&pts[0], nr_points * 3, 0);
435  points->SetData (data);
436  }
437  // Need to check for NaNs, Infs, ec
438  else
439  {
440  for (vtkIdType i = 0; i < nr_points; ++i)
441  {
442  // Check if the point is invalid
443  if (!pcl_isfinite (cloud_->points[i].x) || !pcl_isfinite (cloud_->points[i].y) || !pcl_isfinite (cloud_->points[i].z))
444  continue;
445 
446  pts[j * 3 + 0] = cloud_->points[i].x;
447  pts[j * 3 + 1] = cloud_->points[i].y;
448  pts[j * 3 + 2] = cloud_->points[i].z;
449  // Set j and increment
450  j++;
451  }
452  data->SetArray (&pts[0], j * 3, 0);
453  points->SetData (data);
454  }
455 }
456 
458 template <typename PointT>
459 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const PointCloudConstPtr &cloud)
460  : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
461 {
462  field_x_idx_ = pcl::getFieldIndex (*cloud, "normal_x", fields_);
463  if (field_x_idx_ == -1)
464  return;
465  field_y_idx_ = pcl::getFieldIndex (*cloud, "normal_y", fields_);
466  if (field_y_idx_ == -1)
467  return;
468  field_z_idx_ = pcl::getFieldIndex (*cloud, "normal_z", fields_);
469  if (field_z_idx_ == -1)
470  return;
471  capable_ = true;
472 }
473 
475 template <typename PointT> void
476 pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
477 {
478  if (!capable_)
479  return;
480 
481  if (!points)
482  points = vtkSmartPointer<vtkPoints>::New ();
483  points->SetDataTypeToFloat ();
484  points->SetNumberOfPoints (cloud_->points.size ());
485 
486  // Add all points
487  double p[3];
488  for (vtkIdType i = 0; i < static_cast<vtkIdType> (cloud_->points.size ()); ++i)
489  {
490  p[0] = cloud_->points[i].normal[0];
491  p[1] = cloud_->points[i].normal[1];
492  p[2] = cloud_->points[i].normal[2];
493 
494  points->SetPoint (i, p);
495  }
496 }
497 
499 template <typename PointT>
500 pcl::visualization::PointCloudGeometryHandlerCustom<PointT>::PointCloudGeometryHandlerCustom (const PointCloudConstPtr &cloud,
501  const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) : pcl::visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
502 {
503  field_x_idx_ = pcl::getFieldIndex (*cloud, x_field_name, fields_);
504  if (field_x_idx_ == -1)
505  return;
506  field_y_idx_ = pcl::getFieldIndex (*cloud, y_field_name, fields_);
507  if (field_y_idx_ == -1)
508  return;
509  field_z_idx_ = pcl::getFieldIndex (*cloud, z_field_name, fields_);
510  if (field_z_idx_ == -1)
511  return;
512  field_name_ = x_field_name + y_field_name + z_field_name;
513  capable_ = true;
514 }
515 
517 template <typename PointT> void
518 pcl::visualization::PointCloudGeometryHandlerCustom<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
519 {
520  if (!capable_)
521  return;
522 
523  if (!points)
524  points = vtkSmartPointer<vtkPoints>::New ();
525  points->SetDataTypeToFloat ();
526  points->SetNumberOfPoints (cloud_->points.size ());
527 
528  float data;
529  // Add all points
530  double p[3];
531  for (vtkIdType i = 0; i < static_cast<vtkIdType> (cloud_->points.size ()); ++i)
532  {
533  // Copy the value at the specified field
534  const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[i]);
535  memcpy (&data, pt_data + fields_[field_x_idx_].offset, sizeof (float));
536  p[0] = data;
537 
538  memcpy (&data, pt_data + fields_[field_y_idx_].offset, sizeof (float));
539  p[1] = data;
540 
541  memcpy (&data, pt_data + fields_[field_z_idx_].offset, sizeof (float));
542  p[2] = data;
543 
544  points->SetPoint (i, p);
545  }
546 }
547 
Generated on Fri Mar 8 2013 12:51:00 for Point Cloud Library (PCL) by   doxygen 1.8.3.1