Point Cloud Library (PCL)  1.7.0
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  uint8_t* pt_data = reinterpret_cast<uint8_t*> (&cloud->points[(*removed_indices_)[rii]]);
62  for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
63  memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
64  }
65  if (!pcl_isfinite (user_filter_value_))
66  cloud->is_dense = false;
67 }
68 
69 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
72 {
73  std::vector<int> indices;
74  if (keep_organized_)
75  {
76  bool temp = extract_removed_indices_;
77  extract_removed_indices_ = true;
78  applyFilterIndices (indices);
79  extract_removed_indices_ = temp;
80 
81  output = *input_;
82  std::vector<pcl::PCLPointField> fields;
83  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
84  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
85  {
86  uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output.points[(*removed_indices_)[rii]]);
87  for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
88  memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
89  }
90  if (!pcl_isfinite (user_filter_value_))
91  output.is_dense = false;
92  }
93  else
94  {
95  applyFilterIndices (indices);
96  copyPointCloud (*input_, indices, output);
97  }
98 }
99 
100 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
101 template <typename PointT> void
103 {
104  if (indices_->size () > input_->points.size ())
105  {
106  PCL_ERROR ("[pcl::%s::applyFilter] The indices size exceeds the size of the input.\n", getClassName ().c_str ());
107  indices.clear ();
108  removed_indices_->clear ();
109  return;
110  }
111 
112  if (!negative_) // Normal functionality
113  {
114  indices = *indices_;
115 
116  if (extract_removed_indices_)
117  {
118  // Set up the full indices set
119  std::vector<int> full_indices (input_->points.size ());
120  for (int fii = 0; fii < static_cast<int> (full_indices.size ()); ++fii) // fii = full indices iterator
121  full_indices[fii] = fii;
122 
123  // Set up the sorted input indices
124  std::vector<int> sorted_input_indices = *indices_;
125  std::sort (sorted_input_indices.begin (), sorted_input_indices.end ());
126 
127  // Store the difference in removed_indices
128  removed_indices_->clear ();
129  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (*removed_indices_, removed_indices_->begin ()));
130  }
131  }
132  else // Inverted functionality
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 indices
144  indices.clear ();
145  set_difference (full_indices.begin (), full_indices.end (), sorted_input_indices.begin (), sorted_input_indices.end (), inserter (indices, indices.begin ()));
146 
147  if (extract_removed_indices_)
148  removed_indices_ = indices_;
149  }
150 }
151 
152 #define PCL_INSTANTIATE_ExtractIndices(T) template class PCL_EXPORTS pcl::ExtractIndices<T>;
153 
154 #endif // PCL_FILTERS_IMPL_EXTRACT_INDICES_HPP_
155