Point Cloud Library (PCL)  1.14.1-dev
extract_indices.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_EXTRACT_INDICES_HPP_
41 #define PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
42 
43 #include <pcl/filters/extract_indices.h>
44 #include <numeric> // for std::iota
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49 {
50  Indices indices;
51  bool temp = extract_removed_indices_;
52  extract_removed_indices_ = true;
53  this->setInputCloud (cloud);
54  applyFilterIndices (indices);
55  extract_removed_indices_ = temp;
56 
57  std::vector<pcl::PCLPointField> fields;
58  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
59  for (const auto& rii : (*removed_indices_)) // rii = removed indices iterator
60  {
61  auto pt_index = static_cast<uindex_t>(rii);
62  if (pt_index >= input_->size ())
63  {
64  PCL_ERROR ("[pcl::%s::filterDirectly] The index exceeds the size of the input. Do nothing.\n",
65  getClassName ().c_str ());
66  *cloud = *input_;
67  return;
68  }
69  auto* pt_data = reinterpret_cast<std::uint8_t*> (&(*cloud)[pt_index]);
70  for (const auto &field : fields)
71  memcpy (pt_data + field.offset, &user_filter_value_, sizeof (float));
72  }
73  if (!std::isfinite (user_filter_value_))
74  cloud->is_dense = false;
75 }
76 
77 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT> void
80 {
81  Indices indices;
82  if (keep_organized_)
83  {
84  bool temp = extract_removed_indices_;
85  extract_removed_indices_ = true;
86  applyFilterIndices (indices);
87  extract_removed_indices_ = temp;
88 
89  output = *input_;
90  std::vector<pcl::PCLPointField> fields;
91  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
92  for (const auto ri : *removed_indices_) // ri = removed index
93  {
94  auto pt_index = static_cast<std::size_t>(ri);
95  if (pt_index >= input_->size ())
96  {
97  PCL_ERROR ("[pcl::%s::applyFilter] The index exceeds the size of the input. Do nothing.\n",
98  getClassName ().c_str ());
99  output = *input_;
100  return;
101  }
102  auto* pt_data = reinterpret_cast<std::uint8_t*> (&output[pt_index]);
103  for (const auto &field : fields)
104  memcpy (pt_data + field.offset, &user_filter_value_, sizeof (float));
105  }
106  if (!std::isfinite (user_filter_value_))
107  output.is_dense = false;
108  }
109  else
110  {
111  applyFilterIndices (indices);
112  copyPointCloud (*input_, indices, output);
113  }
114 }
115 
116 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename PointT> void
119 {
120  if (indices_->size () > input_->size ())
121  {
122  PCL_ERROR ("[pcl::%s::applyFilter] The indices size exceeds the size of the input.\n", getClassName ().c_str ());
123  indices.clear ();
124  removed_indices_->clear ();
125  return;
126  }
127 
128  if (!negative_) // Normal functionality
129  {
130  indices = *indices_;
131 
132  if (extract_removed_indices_)
133  {
134  // Set up the full indices set
135  Indices full_indices (input_->size ());
136  std::iota (full_indices.begin (), full_indices.end (), static_cast<index_t> (0));
137 
138  // Set up the sorted input indices
139  Indices sorted_input_indices = *indices_;
140  std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
141 
142  // Store the difference in removed_indices
143  removed_indices_->clear ();
144  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (*removed_indices_, removed_indices_->begin ()));
145  }
146  }
147  else // Inverted functionality
148  {
149  // Set up the full indices set
150  Indices full_indices (input_->size ());
151  std::iota (full_indices.begin (), full_indices.end (), static_cast<index_t> (0));
152 
153  // Set up the sorted input indices
154  Indices sorted_input_indices = *indices_;
155  std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
156 
157  // Store the difference in indices
158  indices.clear ();
159  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (indices, indices.begin ()));
160 
161  if (extract_removed_indices_)
162  removed_indices_ = indices_;
163  }
164 }
165 
166 #define PCL_INSTANTIATE_ExtractIndices(T) template class PCL_EXPORTS pcl::ExtractIndices<T>;
167 
168 #endif // PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
169 
void filterDirectly(PointCloudPtr &cloud)
Apply the filter and store the results directly in the input cloud.
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
void applyFilter(PointCloud &output) override
Filtered results are stored in a separate point cloud.
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
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:403
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:142
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133