41 #include <pcl/filters/impl/extract_indices.hpp> 45 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
53 for (
size_t i = 0; i < indices_->size (); ++i)
54 for (
size_t j = 0; j < output.fields.size(); ++j)
55 memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
56 &user_filter_value_,
sizeof(
float));
61 std::vector<int> all_indices (input_->width * input_->height);
62 for (
int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
65 std::vector<int> indices = *indices_;
66 std::sort (indices.begin (), indices.end ());
69 std::vector<int> remaining_indices;
70 set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
71 inserter (remaining_indices, remaining_indices.begin ()));
74 for (
size_t i = 0; i < remaining_indices.size (); ++i)
75 for (
size_t j = 0; j < output.fields.size(); ++j)
76 memcpy (&output.data[remaining_indices[i] * output.point_step + output.fields[j].offset],
77 &user_filter_value_,
sizeof(
float));
79 if (!pcl_isfinite (user_filter_value_))
80 output.is_dense =
false;
83 if (indices_->empty () || (input_->width * input_->height == 0))
85 output.width = output.height = 0;
92 if (indices_->size () == (input_->width * input_->height))
97 output.width = output.height = 0;
107 output.is_bigendian = input_->is_bigendian;
108 output.point_step = input_->point_step;
111 output.is_dense =
false;
116 std::vector<int> all_indices (input_->width * input_->height);
117 for (
int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
120 std::vector<int> indices = *indices_;
121 std::sort (indices.begin (), indices.end ());
124 std::vector<int> remaining_indices;
125 set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
126 inserter (remaining_indices, remaining_indices.begin ()));
129 output.width =
static_cast<uint32_t
> (remaining_indices.size ());
130 output.data.resize (remaining_indices.size () * output.point_step);
131 for (
size_t i = 0; i < remaining_indices.size (); ++i)
132 memcpy (&output.data[i * output.point_step], &input_->data[remaining_indices[i] * output.point_step], output.point_step);
137 output.width =
static_cast<uint32_t
> (indices_->size ());
138 output.data.resize (indices_->size () * output.point_step);
139 for (
size_t i = 0; i < indices_->size (); ++i)
140 memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * output.point_step], output.point_step);
142 output.row_step = output.point_step * output.width;
147 pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (std::vector<int> &indices)
152 if (indices_->size () == (input_->width * input_->height))
160 std::vector<int> indices_fullset (input_->width * input_->height);
161 for (
int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
162 indices_fullset[p_it] = p_it;
165 if (indices_->empty () || (input_->width * input_->height == 0))
168 indices = indices_fullset;
174 std::vector<int> indices_subset = *indices_;
175 std::sort (indices_subset.begin (), indices_subset.end ());
178 set_difference (indices_fullset.begin (), indices_fullset.end (), indices_subset.begin (), indices_subset.end (), inserter (indices, indices.begin ()));
184 #ifndef PCL_NO_PRECOMPILE 185 #include <pcl/impl/instantiate.hpp> 186 #include <pcl/point_types.h> 188 #ifdef PCL_ONLY_CORE_POINT_TYPES 189 PCL_INSTANTIATE(
ExtractIndices, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::Normal)(pcl::PointXYZRGBNormal))
194 #endif // PCL_NO_PRECOMPILE