38 #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
39 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
42 #include <pcl/common/point_tests.h>
43 #include <pcl/filters/voxel_grid_covariance.h>
44 #include <Eigen/Cholesky>
45 #include <Eigen/Eigenvalues>
46 #include <boost/mpl/size.hpp>
47 #include <boost/random/mersenne_twister.hpp>
48 #include <boost/random/normal_distribution.hpp>
49 #include <boost/random/variate_generator.hpp>
52 template<
typename Po
intT>
void
55 voxel_centroids_leaf_indices_.clear ();
60 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
71 Eigen::Vector4f min_p, max_p;
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_);
76 getMinMax3D<PointT> (*input_, min_p, max_p);
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.", 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);
108 int centroid_size = 4;
110 if (downsample_all_data_)
111 centroid_size = boost::mpl::size<FieldList>::value;
114 std::vector<pcl::PCLPointField> fields;
116 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
117 if (rgba_index == -1)
118 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
121 rgba_index = fields[rgba_index].offset;
126 if (!filter_field_name_.empty ())
129 std::vector<pcl::PCLPointField> fields;
130 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
131 if (distance_idx == -1)
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);
144 float distance_value = 0;
145 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
147 if (filter_limit_negative_)
150 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
156 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
161 const Eigen::Vector4i ijk =
162 Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
163 .template cast<int>();
165 int idx = (ijk - min_b_).dot(divb_mul_);
167 Leaf& leaf = leaves_[idx];
168 if (leaf.nr_points == 0)
170 leaf.centroid.resize (centroid_size);
171 leaf.centroid.setZero ();
174 Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
178 leaf.cov_ += pt3d * pt3d.transpose ();
181 if (!downsample_all_data_)
183 leaf.centroid.template head<3> () += point.getVector3fMap();
188 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
194 const pcl::RGB& rgb = *
reinterpret_cast<const RGB*
> (
reinterpret_cast<const char*
> (&point) + rgba_index);
195 centroid[centroid_size - 4] = rgb.a;
196 centroid[centroid_size - 3] = rgb.r;
197 centroid[centroid_size - 2] = rgb.g;
198 centroid[centroid_size - 1] = rgb.b;
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())
219 .template cast<int>();
221 int idx = (ijk - min_b_).dot(divb_mul_);
223 Leaf& leaf = leaves_[idx];
224 if (leaf.nr_points == 0)
226 leaf.centroid.resize (centroid_size);
227 leaf.centroid.setZero ();
230 Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
234 leaf.cov_ += pt3d * pt3d.transpose ();
237 if (!downsample_all_data_)
239 leaf.centroid.template head<3> () += point.getVector3fMap();
244 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
250 const pcl::RGB& rgb = *
reinterpret_cast<const RGB*
> (
reinterpret_cast<const char*
> (&point) + rgba_index);
251 centroid[centroid_size - 4] = rgb.a;
252 centroid[centroid_size - 3] = rgb.r;
253 centroid[centroid_size - 2] = rgb.g;
254 centroid[centroid_size - 1] = rgb.b;
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;
272 Eigen::Matrix3d eigen_val;
273 Eigen::Vector3d pt_sum;
276 double min_covar_eigvalue;
278 for (
typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
282 Leaf& leaf = it->second;
285 leaf.centroid /=
static_cast<float> (leaf.nr_points);
289 leaf.mean_ /= 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_)
303 output.
back ().x = leaf.centroid[0];
304 output.
back ().y = leaf.centroid[1];
305 output.
back ().z = leaf.centroid[2];
313 pcl::RGB& rgb = *
reinterpret_cast<RGB*
> (
reinterpret_cast<char*
> (&output.
back ()) + rgba_index);
314 rgb.a = leaf.centroid[centroid_size - 4];
315 rgb.r = leaf.centroid[centroid_size - 3];
316 rgb.g = leaf.centroid[centroid_size - 2];
317 rgb.b = leaf.centroid[centroid_size - 1];
323 voxel_centroids_leaf_indices_.push_back (
static_cast<int> (it->first));
326 leaf.cov_ = (leaf.cov_ - pt_sum * leaf.mean_.transpose()) / (leaf.nr_points - 1.0);
329 eigensolver.compute (leaf.cov_);
330 eigen_val = eigensolver.eigenvalues ().asDiagonal ();
331 leaf.evecs_ = eigensolver.eigenvectors ();
333 if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
341 min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
342 if (eigen_val (0, 0) < min_covar_eigvalue)
344 eigen_val (0, 0) = min_covar_eigvalue;
346 if (eigen_val (1, 1) < min_covar_eigvalue)
348 eigen_val (1, 1) = min_covar_eigvalue;
351 leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
353 leaf.evals_ = eigen_val.diagonal ();
355 leaf.icov_ = leaf.cov_.inverse ();
356 if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
357 || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
369 template<
typename Po
intT>
int
375 Eigen::Vector4i ijk = Eigen::floor(reference_point.getArray4fMap() * inverse_leaf_size_).template cast<int>();
377 const Eigen::Array4i diff2min = min_b_ - ijk;
378 const Eigen::Array4i diff2max = max_b_ - ijk;
379 neighbors.reserve (relative_coordinates.cols ());
382 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
384 const Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
386 if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
388 const auto leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
389 if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
392 neighbors.push_back (leaf);
397 return static_cast<int> (neighbors.size());
401 template<
typename Po
intT>
int
405 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
409 template<
typename Po
intT>
int
412 return getNeighborhoodAtPoint(Eigen::Matrix<int, 3, Eigen::Dynamic>::Zero(3,1), reference_point, neighbors);
416 template<
typename Po
intT>
int
419 Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 7);
420 relative_coordinates.setZero();
421 relative_coordinates(0, 1) = 1;
422 relative_coordinates(0, 2) = -1;
423 relative_coordinates(1, 3) = 1;
424 relative_coordinates(1, 4) = -1;
425 relative_coordinates(2, 5) = 1;
426 relative_coordinates(2, 6) = -1;
428 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
432 template<
typename Po
intT>
int
435 Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 27);
436 relative_coordinates.col(0).setZero();
439 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
443 template<
typename Po
intT>
void
448 int pnt_per_cell = 1000;
450 boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
451 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
453 Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
454 Eigen::Matrix3d cholesky_decomp;
455 Eigen::Vector3d cell_mean;
456 Eigen::Vector3d rand_point;
457 Eigen::Vector3d dist_point;
460 for (
typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
462 Leaf& leaf = it->second;
464 if (leaf.nr_points >= min_points_per_voxel_)
466 cell_mean = leaf.mean_;
467 llt_of_cov.compute (leaf.cov_);
468 cholesky_decomp = llt_of_cov.matrixL ();
471 for (
int i = 0; i < pnt_per_cell; i++)
473 rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
474 dist_point = cell_mean + cholesky_decomp * rand_point;
475 cell_cloud.
push_back (
PointXYZ (
static_cast<float> (dist_point (0)),
static_cast<float> (dist_point (1)),
static_cast<float> (dist_point (2))));
481 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
483 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_