40 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
41 #define PCL_FILTERS_VOXEL_GRID_MAP_H_
43 #include <pcl/filters/boost.h>
44 #include <pcl/filters/filter.h>
59 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
77 const std::string &distance_field_name,
float min_distance,
float max_distance,
78 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
84 inline Eigen::MatrixXi
87 Eigen::MatrixXi relative_coordinates (3, 13);
91 for (
int i = -1; i < 2; i++)
93 for (
int j = -1; j < 2; j++)
95 relative_coordinates (0, idx) = i;
96 relative_coordinates (1, idx) = j;
97 relative_coordinates (2, idx) = -1;
102 for (
int i = -1; i < 2; i++)
104 relative_coordinates (0, idx) = i;
105 relative_coordinates (1, idx) = -1;
106 relative_coordinates (2, idx) = 0;
110 relative_coordinates (0, idx) = -1;
111 relative_coordinates (1, idx) = 0;
112 relative_coordinates (2, idx) = 0;
114 return (relative_coordinates);
121 inline Eigen::MatrixXi
125 Eigen::MatrixXi relative_coordinates_all( 3, 26);
126 relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
127 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
128 return (relative_coordinates_all);
142 template <
typename Po
intT>
void
144 const std::string &distance_field_name,
float min_distance,
float max_distance,
145 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
159 template <
typename Po
intT>
void
161 const std::vector<int> &indices,
162 const std::string &distance_field_name,
float min_distance,
float max_distance,
163 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
bool limit_negative =
false);
177 template <
typename Po
intT>
189 typedef boost::shared_ptr< VoxelGrid<PointT> >
Ptr;
190 typedef boost::shared_ptr< const VoxelGrid<PointT> >
ConstPtr;
201 min_b_ (Eigen::Vector4i::Zero ()),
202 max_b_ (Eigen::Vector4i::Zero ()),
203 div_b_ (Eigen::Vector4i::Zero ()),
249 inline Eigen::Vector3f
277 inline Eigen::Vector3i
283 inline Eigen::Vector3i
289 inline Eigen::Vector3i
295 inline Eigen::Vector3i
320 inline std::vector<int>
323 Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x *
inverse_leaf_size_[0])),
326 Eigen::Array4i diff2min =
min_b_ - ijk;
327 Eigen::Array4i diff2max =
max_b_ - ijk;
328 std::vector<int> neighbors (relative_coordinates.cols());
329 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
331 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
333 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
344 inline std::vector<int>
352 inline Eigen::Vector3i
356 static_cast<int> (floor (y * inverse_leaf_size_[1])),
357 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
366 int idx = ((Eigen::Vector4i() << ijk, 0).finished() -
min_b_).dot (
divb_mul_);
367 if (idx < 0 || idx >= static_cast<int> (
leaf_layout_.size ()))
387 inline std::string
const
508 leaf_size_ (Eigen::Vector4f::Zero ()),
509 inverse_leaf_size_ (Eigen::Array4f::Zero ()),
510 downsample_all_data_ (true),
511 save_leaf_layout_ (false),
513 min_b_ (Eigen::Vector4i::Zero ()),
514 max_b_ (Eigen::Vector4i::Zero ()),
515 div_b_ (Eigen::Vector4i::Zero ()),
516 divb_mul_ (Eigen::Vector4i::Zero ()),
517 filter_field_name_ (
""),
518 filter_limit_min_ (-FLT_MAX),
519 filter_limit_max_ (FLT_MAX),
520 filter_limit_negative_ (false)
522 filter_name_ =
"VoxelGrid";
534 setLeafSize (
const Eigen::Vector4f &leaf_size)
536 leaf_size_ = leaf_size;
538 if (leaf_size_[3] == 0)
541 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
550 setLeafSize (
float lx,
float ly,
float lz)
552 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
554 if (leaf_size_[3] == 0)
557 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
561 inline Eigen::Vector3f
589 inline Eigen::Vector3i
595 inline Eigen::Vector3i
601 inline Eigen::Vector3i
607 inline Eigen::Vector3i
618 getCentroidIndex (
float x,
float y,
float z)
620 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
621 static_cast<int> (floor (y * inverse_leaf_size_[1])),
622 static_cast<int> (floor (z * inverse_leaf_size_[2])),
624 - min_b_).dot (divb_mul_)));
635 inline std::vector<int>
636 getNeighborCentroidIndices (
float x,
float y,
float z,
const Eigen::MatrixXi &relative_coordinates)
638 Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
639 static_cast<int> (floor (y * inverse_leaf_size_[1])),
640 static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
641 Eigen::Array4i diff2min = min_b_ - ijk;
642 Eigen::Array4i diff2max = max_b_ - ijk;
643 std::vector<int> neighbors (relative_coordinates.cols());
644 for (
int ni = 0; ni < relative_coordinates.cols (); ni++)
646 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
648 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
649 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))];
664 inline std::vector<int>
665 getNeighborCentroidIndices (
float x,
float y,
float z,
const std::vector<Eigen::Vector3i> &relative_coordinates)
667 Eigen::Vector4i ijk (static_cast<int> (floorf (x * inverse_leaf_size_[0])), static_cast<int> (floorf (y * inverse_leaf_size_[1])), static_cast<int> (floorf (z * inverse_leaf_size_[2])), 0);
668 std::vector<int> neighbors;
669 neighbors.reserve (relative_coordinates.size ());
670 for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
671 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
678 inline std::vector<int>
686 inline Eigen::Vector3i
687 getGridCoordinates (
float x,
float y,
float z)
689 return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
690 static_cast<int> (floor (y * inverse_leaf_size_[1])),
691 static_cast<int> (floor (z * inverse_leaf_size_[2]))));
698 getCentroidIndexAt (
const Eigen::Vector3i &ijk)
700 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
701 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ()))
707 return (leaf_layout_[idx]);
715 setFilterFieldName (
const std::string &field_name)
717 filter_field_name_ = field_name;
721 inline std::string
const
722 getFilterFieldName ()
724 return (filter_field_name_);
732 setFilterLimits (
const double &limit_min,
const double &limit_max)
734 filter_limit_min_ = limit_min;
735 filter_limit_max_ = limit_max;
743 getFilterLimits (
double &limit_min,
double &limit_max)
745 limit_min = filter_limit_min_;
746 limit_max = filter_limit_max_;
754 setFilterLimitsNegative (
const bool limit_negative)
756 filter_limit_negative_ = limit_negative;
763 getFilterLimitsNegative (
bool &limit_negative)
765 limit_negative = filter_limit_negative_;
772 getFilterLimitsNegative ()
774 return (filter_limit_negative_);
800 Eigen::Vector4i
min_b_, max_b_, div_b_, divb_mul_;
822 #ifdef PCL_NO_PRECOMPILE
823 #include <pcl/filters/impl/voxel_grid.hpp>
826 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_