38 #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
39 #define PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
41 #include <pcl/common/common.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);
75 int64_t dx =
static_cast<int64_t
>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
76 int64_t dy =
static_cast<int64_t
>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
77 int64_t dz =
static_cast<int64_t
>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
79 if((dx*dy*dz) > std::numeric_limits<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> (floor (min_p[0] * inverse_leaf_size_[0]));
88 max_b_[0] =
static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
89 min_b_[1] =
static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
90 max_b_[1] =
static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
91 min_b_[2] =
static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
92 max_b_[2] =
static_cast<int> (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;
113 if (rgba_index == -1)
117 rgba_index = fields[rgba_index].offset;
122 if (!filter_field_name_.empty ())
125 std::vector<pcl::PCLPointField> 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 (
size_t cp = 0; cp < input_->points.size (); ++cp)
133 if (!input_->is_dense)
135 if (!pcl_isfinite (input_->points[cp].x) ||
136 !pcl_isfinite (input_->points[cp].y) ||
137 !pcl_isfinite (input_->points[cp].z))
141 const uint8_t* pt_data =
reinterpret_cast<const uint8_t*
> (&input_->points[cp]);
142 float distance_value = 0;
143 memcpy (&distance_value, pt_data + fields[distance_idx].offset,
sizeof (
float));
145 if (filter_limit_negative_)
148 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
154 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
158 int ijk0 =
static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
159 int ijk1 =
static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
160 int ijk2 =
static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
163 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
165 Leaf& leaf = leaves_[idx];
168 leaf.
centroid.resize (centroid_size);
172 Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
176 leaf.
cov_ += pt3d * pt3d.transpose ();
179 if (!downsample_all_data_)
181 Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
182 leaf.
centroid.template head<4> () += pt;
187 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
193 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (
int));
194 centroid[centroid_size - 3] =
static_cast<float> ((rgb >> 16) & 0x0000ff);
195 centroid[centroid_size - 2] =
static_cast<float> ((rgb >> 8) & 0x0000ff);
196 centroid[centroid_size - 1] =
static_cast<float> ((rgb) & 0x0000ff);
208 for (
size_t cp = 0; cp < input_->points.size (); ++cp)
210 if (!input_->is_dense)
212 if (!pcl_isfinite (input_->points[cp].x) ||
213 !pcl_isfinite (input_->points[cp].y) ||
214 !pcl_isfinite (input_->points[cp].z))
217 int ijk0 =
static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) -
static_cast<float> (min_b_[0]));
218 int ijk1 =
static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) -
static_cast<float> (min_b_[1]));
219 int ijk2 =
static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) -
static_cast<float> (min_b_[2]));
222 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
225 Leaf& leaf = leaves_[idx];
228 leaf.
centroid.resize (centroid_size);
232 Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
236 leaf.
cov_ += pt3d * pt3d.transpose ();
239 if (!downsample_all_data_)
241 Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
242 leaf.
centroid.template head<4> () += pt;
247 Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
253 memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (
int));
254 centroid[centroid_size - 3] =
static_cast<float> ((rgb >> 16) & 0x0000ff);
255 centroid[centroid_size - 2] =
static_cast<float> ((rgb >> 8) & 0x0000ff);
256 centroid[centroid_size - 1] =
static_cast<float> ((rgb) & 0x0000ff);
266 output.
points.reserve (leaves_.size ());
268 voxel_centroids_leaf_indices_.reserve (leaves_.size ());
270 if (save_leaf_layout_)
271 leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
274 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
275 Eigen::Matrix3d eigen_val;
276 Eigen::Vector3d pt_sum;
279 double min_covar_eigvalue;
281 for (
typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
285 Leaf& leaf = it->second;
296 if (leaf.
nr_points >= min_points_per_voxel_)
298 if (save_leaf_layout_)
299 leaf_layout_[it->first] = cp++;
304 if (!downsample_all_data_)
317 float r = leaf.
centroid[centroid_size - 3], g = leaf.
centroid[centroid_size - 2], b = leaf.
centroid[centroid_size - 1];
318 int rgb = (
static_cast<int> (r)) << 16 | (
static_cast<int> (g)) << 8 | (
static_cast<int> (b));
319 memcpy (reinterpret_cast<char*> (&output.
points.back ()) + rgba_index, &rgb,
sizeof (
float));
325 voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
332 eigensolver.compute (leaf.
cov_);
333 eigen_val = eigensolver.eigenvalues ().asDiagonal ();
334 leaf.
evecs_ = eigensolver.eigenvectors ();
336 if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
344 min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
345 if (eigen_val (0, 0) < min_covar_eigvalue)
347 eigen_val (0, 0) = min_covar_eigvalue;
349 if (eigen_val (1, 1) < min_covar_eigvalue)
351 eigen_val (1, 1) = min_covar_eigvalue;
356 leaf.
evals_ = eigen_val.diagonal ();
359 if (leaf.
icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
360 || leaf.
icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
368 output.
width =
static_cast<uint32_t
> (output.
points.size ());
372 template<
typename Po
intT>
int
379 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x / leaf_size_[0])),
380 static_cast<int> (floor (reference_point.y / leaf_size_[1])),
381 static_cast<int> (floor (reference_point.z / leaf_size_[2])), 0);
382 Eigen::Array4i diff2min = min_b_ - ijk;
383 Eigen::Array4i diff2max = max_b_ - ijk;
384 neighbors.reserve (relative_coordinates.cols ());
388 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
390 Eigen::Vector4i displacement = (Eigen::Vector4i () << relative_coordinates.col (ni), 0).finished ();
392 if ((diff2min <= displacement.array ()).all () && (diff2max >= displacement.array ()).all ())
394 typename std::map<size_t, Leaf>::iterator leaf_iter = leaves_.find (((ijk + displacement - min_b_).dot (divb_mul_)));
395 if (leaf_iter != leaves_.end () && leaf_iter->second.nr_points >= min_points_per_voxel_)
398 neighbors.push_back (leaf);
403 return (static_cast<int> (neighbors.size ()));
407 template<
typename Po
intT>
void
412 int pnt_per_cell = 1000;
414 boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
415 boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
417 Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
418 Eigen::Matrix3d cholesky_decomp;
419 Eigen::Vector3d cell_mean;
420 Eigen::Vector3d rand_point;
421 Eigen::Vector3d dist_point;
424 for (
typename std::map<size_t, Leaf>::iterator it = leaves_.begin (); it != leaves_.end (); ++it)
426 Leaf& leaf = it->second;
428 if (leaf.
nr_points >= min_points_per_voxel_)
430 cell_mean = leaf.
mean_;
431 llt_of_cov.compute (leaf.
cov_);
432 cholesky_decomp = llt_of_cov.matrixL ();
435 for (
int i = 0; i < pnt_per_cell; i++)
437 rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
438 dist_point = cell_mean + cholesky_decomp * rand_point;
439 cell_cloud.
push_back (
PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
445 #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
447 #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
Eigen::Vector3d mean_
3D voxel centroid
Simple structure to hold a centroid, covarince and the number of points in a leaf.
Eigen::Matrix3d cov_
Voxel covariance matrix.
uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector3d evals_
Eigen values of voxel covariance matrix.
Helper functor structure for copying data between an Eigen type and a PointT.
int getNeighborhoodAtPoint(const PointT &reference_point, std::vector< LeafConstPtr > &neighbors)
Get the voxels surrounding point p, not including the voxel contating point p.
void applyFilter(PointCloud &output)
Filter cloud and initializes voxel structure.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Eigen::Matrix3d icov_
Inverse of voxel covariance matrix.
Eigen::VectorXf centroid
Nd voxel centroid.
const PointT & back() const
Helper functor structure for copying data between an Eigen type and a PointT.
Eigen::Matrix3d evecs_
Eigen vectors of voxel covariance matrix.
A point structure representing Euclidean xyz coordinates, and the RGB color.
uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getDisplayCloud(pcl::PointCloud< PointXYZ > &cell_cloud)
Get a cloud to visualize each voxels normal distribution.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
A point structure representing Euclidean xyz coordinates.
void clear()
Removes all points in a cloud and sets the width and height to 0.
int nr_points
Number of points contained by voxel.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.