Point Cloud Library (PCL)  1.8.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 <pcl/common/io.h>
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49 {
50  std::vector<int> 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 (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
60  {
61  size_t pt_index = (size_t) (*removed_indices_)[rii];
62  if (pt_index >= input_->points.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  uint8_t* pt_data = reinterpret_cast<uint8_t*> (&cloud->points[pt_index]);
70  for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
71  memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
72  }
73  if (!pcl_isfinite (user_filter_value_))
74  cloud->is_dense = false;
75 }
76 
77 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT> void
80 {
81  std::vector<int> 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 (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
93  {
94  size_t pt_index = (size_t)(*removed_indices_)[rii];
95  if (pt_index >= input_->points.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  uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output.points[pt_index]);
103  for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
104  memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
105  }
106  if (!pcl_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_->points.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  std::vector<int> full_indices (input_->points.size ());
136  for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii) // fii = full indices iterator
137  full_indices[fii] = fii;
138 
139  // Set up the sorted input indices
140  std::vector<int> sorted_input_indices = *indices_;
141  std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
142 
143  // Store the difference in removed_indices
144  removed_indices_->clear ();
145  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (*removed_indices_, removed_indices_->begin ()));
146  }
147  }
148  else // Inverted functionality
149  {
150  // Set up the full indices set
151  std::vector<int> full_indices (input_->points.size ());
152  for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii) // fii = full indices iterator
153  full_indices[fii] = fii;
154 
155  // Set up the sorted input indices
156  std::vector<int> sorted_input_indices = *indices_;
157  std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
158 
159  // Store the difference in indices
160  indices.clear ();
161  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (indices, indices.begin ()));
162 
163  if (extract_removed_indices_)
164  removed_indices_ = indices_;
165  }
166 }
167 
168 #define PCL_INSTANTIATE_ExtractIndices(T) template class PCL_EXPORTS pcl::ExtractIndices<T>;
169 
170 #endif // PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
171 
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void applyFilter(PointCloud &output)
Filtered results are stored in a separate point cloud.
PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:72
void applyFilterIndices(std::vector< int > &indices)
Filtered results are indexed by an indices array.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
void filterDirectly(PointCloudPtr &cloud)
Apply the filter and store the results directly in the input cloud.