Point Cloud Library (PCL)  1.14.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 
45 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointT> void
48 {
49  // The arrays to be used
50  indices.resize (indices_->size ());
51  removed_indices_->resize (indices_->size ());
52  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
53 
54  // Has a field name been specified?
55  if (filter_field_name_.empty ())
56  {
57  // Only filter for non-finite entries then
58  for (const auto ii : *indices_) // ii = input index
59  {
60  // Non-finite entries are always passed to removed indices
61  if (!std::isfinite ((*input_)[ii].x) ||
62  !std::isfinite ((*input_)[ii].y) ||
63  !std::isfinite ((*input_)[ii].z))
64  {
65  if (extract_removed_indices_)
66  (*removed_indices_)[rii++] = ii;
67  continue;
68  }
69  indices[oii++] = ii;
70  }
71  }
72  else
73  {
74  // Attempt to get the field name's index
75  std::vector<pcl::PCLPointField> fields;
76  int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
77  if (distance_idx == -1)
78  {
79  PCL_WARN ("[pcl::%s::applyFilter] Unable to find field name in point type.\n", getClassName ().c_str ());
80  indices.clear ();
81  removed_indices_->clear ();
82  return;
83  }
84  if (fields[distance_idx].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32)
85  {
86  PCL_ERROR ("[pcl::%s::applyFilter] PassThrough currently only works with float32 fields. To filter fields of other types see ConditionalRemoval or FunctorFilter/FunctionFilter.\n", getClassName ().c_str ());
87  indices.clear ();
88  removed_indices_->clear ();
89  return;
90  }
91  if (filter_field_name_ == "rgb")
92  PCL_WARN ("[pcl::%s::applyFilter] You told PassThrough to operate on the 'rgb' field. This will likely not do what you expect. Consider using ConditionalRemoval or FunctorFilter/FunctionFilter.\n", getClassName ().c_str ());
93  const auto field_offset = fields[distance_idx].offset;
94 
95  // Filter for non-finite entries and the specified field limits
96  for (const auto ii : *indices_) // ii = input index
97  {
98  // Non-finite entries are always passed to removed indices
99  if (!std::isfinite ((*input_)[ii].x) ||
100  !std::isfinite ((*input_)[ii].y) ||
101  !std::isfinite ((*input_)[ii].z))
102  {
103  if (extract_removed_indices_)
104  (*removed_indices_)[rii++] = ii;
105  continue;
106  }
107 
108  // Get the field's value
109  const auto* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[ii]);
110  float field_value = 0;
111  memcpy (&field_value, pt_data + field_offset, sizeof (float));
112 
113  // Remove NAN/INF/-INF values. We expect passthrough to output clean valid data.
114  if (!std::isfinite (field_value))
115  {
116  if (extract_removed_indices_)
117  (*removed_indices_)[rii++] = ii;
118  continue;
119  }
120 
121  // Outside of the field limits are passed to removed indices
122  if (!negative_ && (field_value < filter_limit_min_ || field_value > filter_limit_max_))
123  {
124  if (extract_removed_indices_)
125  (*removed_indices_)[rii++] = ii;
126  continue;
127  }
128 
129  // Inside of the field limits are passed to removed indices if negative was set
130  if (negative_ && field_value >= filter_limit_min_ && field_value <= filter_limit_max_)
131  {
132  if (extract_removed_indices_)
133  (*removed_indices_)[rii++] = ii;
134  continue;
135  }
136 
137  // Otherwise it was a normal point for output (inlier)
138  indices[oii++] = ii;
139  }
140  }
141 
142  // Resize the output arrays
143  indices.resize (oii);
144  removed_indices_->resize (rii);
145 }
146 
147 #define PCL_INSTANTIATE_PassThrough(T) template class PCL_EXPORTS pcl::PassThrough<T>;
148 
149 #endif // PCL_FILTERS_IMPL_PASSTHROUGH_HPP_
150 
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
Definition: passthrough.hpp:47
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133