38 #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 39 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ 42 #include <pcl/filters/boost.h> 43 #include <pcl/filters/voxel_grid_covariance.h> 44 #include <Eigen/Dense> 45 #include <Eigen/Cholesky> 48 template<
typename Po
intT>
void 51 voxel_centroids_leaf_indices_.clear ();
56 PCL_WARN (
"[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
67 Eigen::Vector4f min_p, max_p;
69 if (!filter_field_name_.empty ())
70 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_);
72 getMinMax3D<PointT> (*input_, min_p, max_p);
79 if((dx*dy*dz) > std::numeric_limits<std::int32_t>::max())
81 PCL_WARN(
"[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
87 min_b_[0] =
static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
88 max_b_[0] =
static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
89 min_b_[1] =
static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
90 max_b_[1] =
static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
91 min_b_[2] =
static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
92 max_b_[2] =
static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
95 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
102 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
104 int centroid_size = 4;
106 if (downsample_all_data_)
107 centroid_size = boost::mpl::size<FieldList>::value;
110 std::vector<pcl::PCLPointField> fields;
112 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
113 if (rgba_index == -1)
114 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
117 rgba_index = fields[rgba_index].offset;
122 if (!filter_field_name_.empty ())
125 std::vector<pcl::PCLPointField> fields;
126 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
127 if (distance_idx == -1)
128 PCL_WARN (
"[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
131 for (
const auto& point: *input_)
133 if (!input_->is_dense)
140 float distance_value = 0;
141 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
143 if (filter_limit_negative_)
146 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
152 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
157 const Eigen::Vector4i ijk =
158 Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
159 .
template cast<int>();
161 int idx = (ijk - min_b_).dot(divb_mul_);
163 Leaf& leaf = leaves_[idx];
164 if (leaf.nr_points == 0)
166 leaf.centroid.resize (centroid_size);
167 leaf.centroid.setZero ();
170 Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
174 leaf.cov_ += pt3d * pt3d.transpose ();
177 if (!downsample_all_data_)
179 leaf.centroid.template head<3> () += point.getVector3fMap();
184 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
190 const pcl::RGB& rgb = *
reinterpret_cast<const RGB*
> (
reinterpret_cast<const char*
> (&point) + rgba_index);
191 centroid[centroid_size - 4] = rgb.a;
192 centroid[centroid_size - 3] = rgb.r;
193 centroid[centroid_size - 2] = rgb.g;
194 centroid[centroid_size - 1] = rgb.b;
196 leaf.centroid += centroid;
205 for (
const auto& point: *input_)
207 if (!input_->is_dense)
213 const Eigen::Vector4i ijk =
214 Eigen::floor(point.getArray4fMap() * inverse_leaf_size_.array())
215 .
template cast<int>();
217 int idx = (ijk - min_b_).dot(divb_mul_);
219 Leaf& leaf = leaves_[idx];
220 if (leaf.nr_points == 0)
222 leaf.centroid.resize (centroid_size);
223 leaf.centroid.setZero ();
226 Eigen::Vector3d pt3d = point.getVector3fMap().template cast<double>();
230 leaf.cov_ += pt3d * pt3d.transpose ();
233 if (!downsample_all_data_)
235 leaf.centroid.template head<3> () += point.getVector3fMap();
240 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
246 const pcl::RGB& rgb = *
reinterpret_cast<const RGB*
> (
reinterpret_cast<const char*
> (&point) + rgba_index);
247 centroid[centroid_size - 4] = rgb.a;
248 centroid[centroid_size - 3] = rgb.r;
249 centroid[centroid_size - 2] = rgb.g;
250 centroid[centroid_size - 1] = rgb.b;
252 leaf.centroid += centroid;
259 output.
points.reserve (leaves_.size ());
261 voxel_centroids_leaf_indices_.reserve (leaves_.size ());
263 if (save_leaf_layout_)
264 leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
267 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
268 Eigen::Matrix3d eigen_val;
269 Eigen::Vector3d pt_sum;
272 double min_covar_eigvalue;
274 for (
typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
278 Leaf& leaf = it->second;
281 leaf.centroid /=
static_cast<float> (leaf.nr_points);
285 leaf.mean_ /= leaf.nr_points;
289 if (leaf.nr_points >= min_points_per_voxel_)
291 if (save_leaf_layout_)
292 leaf_layout_[it->first] = cp++;
297 if (!downsample_all_data_)
299 output.
points.back ().x = leaf.centroid[0];
300 output.
points.back ().y = leaf.centroid[1];
301 output.
points.back ().z = leaf.centroid[2];
309 pcl::RGB& rgb = *
reinterpret_cast<RGB*
> (
reinterpret_cast<char*
> (&output.
points.back ()) + rgba_index);
310 rgb.a = leaf.centroid[centroid_size - 4];
311 rgb.r = leaf.centroid[centroid_size - 3];
312 rgb.g = leaf.centroid[centroid_size - 2];
313 rgb.b = leaf.centroid[centroid_size - 1];
319 voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
322 leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
323 leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
326 eigensolver.compute (leaf.cov_);
327 eigen_val = eigensolver.eigenvalues ().asDiagonal ();
328 leaf.evecs_ = eigensolver.eigenvectors ();
330 if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
338 min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
339 if (eigen_val (0, 0) < min_covar_eigvalue)
341 eigen_val (0, 0) = min_covar_eigvalue;
343 if (eigen_val (1, 1) < min_covar_eigvalue)
345 eigen_val (1, 1) = min_covar_eigvalue;
348 leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
350 leaf.evals_ = eigen_val.diagonal ();
352 leaf.icov_ = leaf.cov_.inverse ();
353 if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
354 || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
366 template<
typename Po
intT>
int 372 Eigen::Vector4i ijk = Eigen::floor(reference_point.getArray4fMap() * inverse_leaf_size_).
template cast<int>();
374 const Eigen::Array4i diff2min = min_b_ - ijk;
375 const Eigen::Array4i diff2max = max_b_ - ijk;
376 neighbors.reserve (relative_coordinates.cols ());
379 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
381 const Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
383 if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
385 const auto leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
386 if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
389 neighbors.push_back (leaf);
394 return static_cast<int> (neighbors.size());
398 template<
typename Po
intT>
int 402 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
406 template<
typename Po
intT>
int 409 return getNeighborhoodAtPoint(Eigen::Matrix<int, 3, Eigen::Dynamic>::Zero(3,1), reference_point, neighbors);
413 template<
typename Po
intT>
int 416 Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 7);
417 relative_coordinates.setZero();
418 relative_coordinates(0, 1) = 1;
419 relative_coordinates(0, 2) = -1;
420 relative_coordinates(1, 3) = 1;
421 relative_coordinates(1, 4) = -1;
422 relative_coordinates(2, 5) = 1;
423 relative_coordinates(2, 6) = -1;
425 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
429 template<
typename Po
intT>
int 432 Eigen::Matrix<int, 3, Eigen::Dynamic> relative_coordinates(3, 27);
433 relative_coordinates.col(0).setZero();
436 return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
440 template<
typename Po
intT>
void 445 int pnt_per_cell = 1000;
447 boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
448 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
450 Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
451 Eigen::Matrix3d cholesky_decomp;
452 Eigen::Vector3d cell_mean;
453 Eigen::Vector3d rand_point;
454 Eigen::Vector3d dist_point;
457 for (
typename std::map<std::size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
459 Leaf& leaf = it->second;
461 if (leaf.nr_points >= min_points_per_voxel_)
463 cell_mean = leaf.mean_;
464 llt_of_cov.compute (leaf.cov_);
465 cholesky_decomp = llt_of_cov.matrixL ();
468 for (
int i = 0; i < pnt_per_cell; i++)
470 rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
471 dist_point = cell_mean + cholesky_decomp * rand_point;
472 cell_cloud.
push_back (
PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
478 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>; 480 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ Helper functor structure for copying data between an Eigen type and a PointT.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Helper functor structure for copying data between an Eigen type and a PointT.
int getFaceNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p and its facing voxels (up to 7 voxels).
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
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.
constexpr bool isXYZFinite(const PointT &) noexcept
int getVoxelAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get the voxel at p.
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
std::uint32_t width
The point cloud width (if organized as an image-structure).
A structure representing RGB color information.
Define standard C methods and C++ classes that are common to all methods.
A point structure representing Euclidean xyz coordinates.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void applyFilter(PointCloud &output) override
Filter cloud and initializes voxel structure.
std::uint32_t height
The point cloud height (if organized as an image-structure).
const PointT & back() const
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
int getAllNeighborsAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors) const
Get all 3x3x3 neighbor voxels of p (up to 27 voxels).
A point structure representing Euclidean xyz coordinates, and the RGB color.
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.