55 voxel_centroids_leaf_indices_.clear ();
60 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().
c_str ());
73 if (!filter_field_name_.empty ())
74 getMinMax3D<PointT> (input_, filter_field_name_,
static_cast<float> (filter_limit_min_),
static_cast<float> (filter_limit_max_),
min_p,
max_p, filter_limit_negative_);
79 std::int64_t dx =
static_cast<std::int64_t
>((
max_p[0] -
min_p[0]) * inverse_leaf_size_[0])+1;
80 std::int64_t
dy =
static_cast<std::int64_t
>((
max_p[1] -
min_p[1]) * inverse_leaf_size_[1])+1;
81 std::int64_t
dz =
static_cast<std::int64_t
>((
max_p[2] -
min_p[2]) * inverse_leaf_size_[2])+1;
83 if((dx*
dy*
dz) > std::numeric_limits<std::int32_t>::max())
85 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().
c_str());
91 min_b_[0] =
static_cast<int> (std::floor (
min_p[0] * inverse_leaf_size_[0]));
92 max_b_[0] =
static_cast<int> (std::floor (
max_p[0] * inverse_leaf_size_[0]));
93 min_b_[1] =
static_cast<int> (std::floor (
min_p[1] * inverse_leaf_size_[1]));
94 max_b_[1] =
static_cast<int> (std::floor (
max_p[1] * inverse_leaf_size_[1]));
95 min_b_[2] =
static_cast<int> (std::floor (
min_p[2] * inverse_leaf_size_[2]));
96 max_b_[2] =
static_cast<int> (std::floor (
max_p[2] * inverse_leaf_size_[2]));
99 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
106 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
110 if (downsample_all_data_)
114 std::vector<pcl::PCLPointField> fields;
126 if (!filter_field_name_.empty ())
129 std::vector<pcl::PCLPointField> fields;
132 PCL_WARN (
"[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().
c_str (),
distance_idx);
135 for (
const auto& point: *input_)
137 if (!input_->is_dense)
143 const std::uint8_t*
pt_data =
reinterpret_cast<const std::uint8_t*
> (&point);
147 if (filter_limit_negative_)
161 const Eigen::Vector4i
ijk =
162 Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
165 int idx = (
ijk - min_b_).dot(divb_mul_);
168 if (
leaf.nr_points == 0)
171 leaf.centroid.setZero ();
181 if (!downsample_all_data_)
183 leaf.centroid.template
head<3> () += point.getVector3fMap();
188 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (
centroid_size);
200 leaf.centroid += centroid;
209 for (
const auto& point: *input_)
211 if (!input_->is_dense)
217 const Eigen::Vector4i
ijk =
218 Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
221 int idx = (
ijk - min_b_).dot(divb_mul_);
224 if (
leaf.nr_points == 0)
227 leaf.centroid.setZero ();
237 if (!downsample_all_data_)
239 leaf.centroid.template
head<3> () += point.getVector3fMap();
244 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (
centroid_size);
256 leaf.centroid += centroid;
263 output.reserve (leaves_.size ());
265 voxel_centroids_leaf_indices_.reserve (leaves_.size ());
267 if (save_leaf_layout_)
268 leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
271 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
eigensolver;
278 for (
typename std::map<std::size_t, Leaf>::iterator
it = leaves_.begin ();
it != leaves_.end (); ++
it)
285 leaf.centroid /=
static_cast<float> (
leaf.nr_points);
293 if (
leaf.nr_points >= min_points_per_voxel_)
295 if (save_leaf_layout_)
296 leaf_layout_[
it->first] = cp++;
301 if (!downsample_all_data_)
323 voxel_centroids_leaf_indices_.push_back (
static_cast<int> (
it->first));
356 if (
leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
357 ||
leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
int getNeighborhoodAtPoint(const Eigen::Matrix< int, 3, Eigen::Dynamic > &relative_coordinates, const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxels surrounding point p designated by #relative_coordinates.