41 #ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 42 #define PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ 44 #include <pcl/filters/voxel_grid.h> 55 template <
typename Po
intT>
98 const Eigen::Vector3i& in_target_voxel);
112 std::vector<Eigen::Vector3i>& out_ray,
113 const Eigen::Vector3i& in_target_voxel);
133 inline Eigen::Vector3f
139 inline Eigen::Vector3f
147 inline Eigen::Vector4f
179 const Eigen::Vector4f& direction);
191 const Eigen::Vector4f& origin,
192 const Eigen::Vector4f& direction,
206 const Eigen::Vector3i& target_voxel,
207 const Eigen::Vector4f& origin,
208 const Eigen::Vector4f& direction,
218 return static_cast<float> (floor (d + 0.5f));
227 inline Eigen::Vector3i
231 static_cast<int> (
round (y * inverse_leaf_size_[1])),
232 static_cast<int> (
round (z * inverse_leaf_size_[2])));
249 #ifdef PCL_NO_PRECOMPILE 250 #include <pcl/filters/impl/voxel_grid_occlusion_estimation.hpp> 253 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_OCCLUSION_ESTIMATION_H_ Eigen::Vector3i getGridCoordinatesRound(float x, float y, float z)
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
float rayBoxIntersection(const Eigen::Vector4f &origin, const Eigen::Vector4f &direction)
Returns the scaling value (tmin) were the ray intersects with the voxel grid bounding box...
Eigen::Vector3f getMaxBoundCoordinates()
Returns the maximum bounding of coordinates of the voxel grid (x,y,z).
virtual ~VoxelGridOcclusionEstimation()
Destructor.
float round(float d)
Returns a rounded value.
int occlusionEstimation(int &out_state, const Eigen::Vector3i &in_target_voxel)
Returns the state (free = 0, occluded = 1) of the voxel after utilizing a ray traversal algorithm to ...
PointCloud filtered_cloud_
Eigen::Quaternionf sensor_orientation_
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Eigen::Vector4f getCentroidCoordinate(const Eigen::Vector3i &ijk)
Returns the corresponding centroid (x,y,z) coordinates in the grid of voxel (i,j,k).
boost::shared_ptr< PointCloud< PointT > > Ptr
PointCloud::ConstPtr PointCloudConstPtr
PointCloud::Ptr PointCloudPtr
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
VoxelGrid to estimate occluded space in the scene.
int occlusionEstimationAll(std::vector< Eigen::Vector3i > &occluded_voxels)
Returns the voxel coordinates (i, j, k) of all occluded voxels in the voxel gird. ...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Eigen::Vector3f getMinBoundCoordinates()
Returns the minimum bounding of coordinates of the voxel grid (x,y,z).
VoxelGridOcclusionEstimation()
Empty constructor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Eigen::Vector4f leaf_size_
The size of a leaf.
Eigen::Vector4f sensor_origin_
Filter< PointT >::PointCloud PointCloud
int rayTraversal(const Eigen::Vector3i &target_voxel, const Eigen::Vector4f &origin, const Eigen::Vector4f &direction, const float t_min)
Returns the state of the target voxel (0 = visible, 1 = occupied) unsing a ray traversal algorithm...
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier...
PointCloud getFilteredPointCloud()
Returns the voxel grid filtered point cloud.
void initializeVoxelGrid()
Initialize the voxel grid, needs to be called first Builts the voxel grid and computes additional val...