Point Cloud Library (PCL)  1.14.1-dev
point_cloud_geometry_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  * $Id: point_cloud_handlers.hpp 7678 2012-10-22 20:54:04Z rusu $
37  *
38  */
39 
40 #ifndef PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_
41 #define PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_
42 
43 #include <pcl/pcl_macros.h>
44 
45 
46 namespace pcl
47 {
48 
49 namespace visualization
50 {
51 
52 template <typename PointT>
55 {
56  field_x_idx_ = pcl::getFieldIndex<PointT> ("x", fields_);
58  return;
59  field_y_idx_ = pcl::getFieldIndex<PointT> ("y", fields_);
61  return;
62  field_z_idx_ = pcl::getFieldIndex<PointT> ("z", fields_);
64  return;
65  capable_ = true;
66 }
67 
68 
69 template <typename PointT> void
71 {
72  if (!capable_)
73  return;
74 
75  if (!points)
77  points->SetDataTypeToFloat ();
78 
80  data->SetNumberOfComponents (3);
81  vtkIdType nr_points = cloud_->size ();
82 
83  // Add all points
84  vtkIdType j = 0; // true point index
85  float* pts = static_cast<float*> (malloc (nr_points * 3 * sizeof (float)));
86 
87  // If the dataset has no invalid values, just copy all of them
88  if (cloud_->is_dense)
89  {
90  for (vtkIdType i = 0; i < nr_points; ++i)
91  {
92  pts[i * 3 + 0] = (*cloud_)[i].x;
93  pts[i * 3 + 1] = (*cloud_)[i].y;
94  pts[i * 3 + 2] = (*cloud_)[i].z;
95  }
96  data->SetArray (&pts[0], nr_points * 3, 0);
97  points->SetData (data);
98  }
99  // Need to check for NaNs, Infs, ec
100  else
101  {
102  for (vtkIdType i = 0; i < nr_points; ++i)
103  {
104  // Check if the point is invalid
105  if (!std::isfinite ((*cloud_)[i].x) || !std::isfinite ((*cloud_)[i].y) || !std::isfinite ((*cloud_)[i].z))
106  continue;
107 
108  pts[j * 3 + 0] = (*cloud_)[i].x;
109  pts[j * 3 + 1] = (*cloud_)[i].y;
110  pts[j * 3 + 2] = (*cloud_)[i].z;
111  // Set j and increment
112  j++;
113  }
114  data->SetArray (&pts[0], j * 3, 0);
115  points->SetData (data);
116  }
117 }
118 
119 
120 template <typename PointT>
123 {
124  field_x_idx_ = pcl::getFieldIndex<PointT> ("normal_x", fields_);
125  if (field_x_idx_ == UNAVAILABLE)
126  return;
127  field_y_idx_ = pcl::getFieldIndex<PointT> ("normal_y", fields_);
128  if (field_y_idx_ == UNAVAILABLE)
129  return;
130  field_z_idx_ = pcl::getFieldIndex<PointT> ("normal_z", fields_);
131  if (field_z_idx_ == UNAVAILABLE)
132  return;
133  capable_ = true;
134 }
135 
136 
137 template <typename PointT> void
139 {
140  if (!capable_)
141  return;
142 
143  if (!points)
145  points->SetDataTypeToFloat ();
146  points->SetNumberOfPoints (cloud_->size ());
147 
148  // Add all points
149  double p[3];
150  for (vtkIdType i = 0; i < static_cast<vtkIdType> (cloud_->size ()); ++i)
151  {
152  p[0] = (*cloud_)[i].normal[0];
153  p[1] = (*cloud_)[i].normal[1];
154  p[2] = (*cloud_)[i].normal[2];
155 
156  points->SetPoint (i, p);
157  }
158 }
159 
160 } // namespace visualization
161 } // namespace pcl
162 
163 #define PCL_INSTANTIATE_PointCloudGeometryHandlerXYZ(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerXYZ<T>;
164 #define PCL_INSTANTIATE_PointCloudGeometryHandlerSurfaceNormal(T) template class PCL_EXPORTS pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<T>;
165 
166 #endif // PCL_POINT_CLOUD_GEOMETRY_HANDLERS_IMPL_HPP_
167 
Base handler class for PointCloud geometry.
std::vector< pcl::PCLPointField > fields_
The list of fields available for this PointCloud.
index_t field_y_idx_
The index of the field holding the Y data.
index_t field_z_idx_
The index of the field holding the Z data.
bool capable_
True if this handler is capable of handling the input data, false otherwise.
index_t field_x_idx_
The index of the field holding the X data.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const
Obtain the actual point geometry for the input dataset in VTK format.
PointCloudGeometryHandlerSurfaceNormal(const PointCloudConstPtr &cloud)
Constructor.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const
Obtain the actual point geometry for the input dataset in VTK format.
PointCloudGeometryHandlerXYZ(const PointCloudConstPtr &cloud)
Constructor.
static constexpr index_t UNAVAILABLE
Definition: pcl_base.h:62
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.