Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47template <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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78template <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////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
117template <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.
typename FilterIndices< PointT >::PointCloud PointCloud
typename PointCloud::Ptr PointCloudPtr
void applyFilter(PointCloud &output) override
Filtered results are stored in a separate point cloud.
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