Point Cloud Library (PCL)  1.9.1-dev
passthrough.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, 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$
37  *
38  */
39 
40 #ifndef PCL_FILTERS_IMPL_PASSTHROUGH_HPP_
41 #define PCL_FILTERS_IMPL_PASSTHROUGH_HPP_
42 
43 #include <pcl/filters/passthrough.h>
44 #include <pcl/common/io.h>
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49 {
50  std::vector<int> indices;
51  if (keep_organized_)
52  {
53  bool temp = extract_removed_indices_;
54  extract_removed_indices_ = true;
55  applyFilterIndices (indices);
56  extract_removed_indices_ = temp;
57 
58  output = *input_;
59  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
60  output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
61  if (!std::isfinite (user_filter_value_))
62  output.is_dense = false;
63  }
64  else
65  {
66  output.is_dense = true;
67  applyFilterIndices (indices);
68  copyPointCloud (*input_, indices, output);
69  }
70 }
71 
72 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointT> void
75 {
76  // The arrays to be used
77  indices.resize (indices_->size ());
78  removed_indices_->resize (indices_->size ());
79  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
80 
81  // Has a field name been specified?
82  if (filter_field_name_.empty ())
83  {
84  // Only filter for non-finite entries then
85  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
86  {
87  // Non-finite entries are always passed to removed indices
88  if (!std::isfinite (input_->points[(*indices_)[iii]].x) ||
89  !std::isfinite (input_->points[(*indices_)[iii]].y) ||
90  !std::isfinite (input_->points[(*indices_)[iii]].z))
91  {
92  if (extract_removed_indices_)
93  (*removed_indices_)[rii++] = (*indices_)[iii];
94  continue;
95  }
96  indices[oii++] = (*indices_)[iii];
97  }
98  }
99  else
100  {
101  // Attempt to get the field name's index
102  std::vector<pcl::PCLPointField> fields;
103  int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
104  if (distance_idx == -1)
105  {
106  PCL_WARN ("[pcl::%s::applyFilter] Unable to find field name in point type.\n", getClassName ().c_str ());
107  indices.clear ();
108  removed_indices_->clear ();
109  return;
110  }
111 
112  // Filter for non-finite entries and the specified field limits
113  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
114  {
115  // Non-finite entries are always passed to removed indices
116  if (!std::isfinite (input_->points[(*indices_)[iii]].x) ||
117  !std::isfinite (input_->points[(*indices_)[iii]].y) ||
118  !std::isfinite (input_->points[(*indices_)[iii]].z))
119  {
120  if (extract_removed_indices_)
121  (*removed_indices_)[rii++] = (*indices_)[iii];
122  continue;
123  }
124 
125  // Get the field's value
126  const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&input_->points[(*indices_)[iii]]);
127  float field_value = 0;
128  memcpy (&field_value, pt_data + fields[distance_idx].offset, sizeof (float));
129 
130  // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data.
131  if (!std::isfinite (field_value))
132  {
133  if (extract_removed_indices_)
134  (*removed_indices_)[rii++] = (*indices_)[iii];
135  continue;
136  }
137 
138  // Outside of the field limits are passed to removed indices
139  if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_))
140  {
141  if (extract_removed_indices_)
142  (*removed_indices_)[rii++] = (*indices_)[iii];
143  continue;
144  }
145 
146  // Inside of the field limits are passed to removed indices if negative was set
147  if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_)
148  {
149  if (extract_removed_indices_)
150  (*removed_indices_)[rii++] = (*indices_)[iii];
151  continue;
152  }
153 
154  // Otherwise it was a normal point for output (inlier)
155  indices[oii++] = (*indices_)[iii];
156  }
157  }
158 
159  // Resize the output arrays
160  indices.resize (oii);
161  removed_indices_->resize (rii);
162 }
163 
164 #define PCL_INSTANTIATE_PassThrough(T) template class PCL_EXPORTS pcl::PassThrough<T>;
165 
166 #endif // PCL_FILTERS_IMPL_PASSTHROUGH_HPP_
167 
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:394
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:402
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
Definition: passthrough.hpp:74
void applyFilter(PointCloud &output) override
Filtered results are stored in a separate point cloud.
Definition: passthrough.hpp:48