From 41ff06f97ede61f0d8fbd371405241fca3b329e3 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 5 Nov 2023 18:18:38 +0100 Subject: [PATCH 1/2] VoxelGridOcclusionEstimation: fix behaviour if sensor origin is inside the voxel grid --- .../filters/impl/voxel_grid_occlusion_estimation.hpp | 10 ++++++++-- .../pcl/filters/voxel_grid_occlusion_estimation.h | 10 +++++++++- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp index 5a6d3974014..63fac68a267 100644 --- a/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp +++ b/filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp @@ -62,6 +62,13 @@ pcl::VoxelGridOcclusionEstimation::initializeVoxelGrid () // set the sensor origin and sensor orientation sensor_origin_ = filtered_cloud_.sensor_origin_; sensor_orientation_ = filtered_cloud_.sensor_orientation_; + + Eigen::Vector3i ijk = this->getGridCoordinates(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z()); + // first check whether the sensor origin is within the voxel grid bounding box, then whether it is occupied + if((ijk[0]>=min_b_[0] && ijk[1]>=min_b_[1] && ijk[2]>=min_b_[2] && ijk[0]<=max_b_[0] && ijk[1]<=max_b_[1] && ijk[2]<=max_b_[2]) && this->getCentroidIndexAt (ijk) != -1) { + PCL_WARN ("[VoxelGridOcclusionEstimation::initializeVoxelGrid] The voxel containing the sensor origin (%g, %g, %g) is marked as occupied. We will instead assume that it is free, to be able to perform the occlusion estimation.\n", sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z()); + this->leaf_layout_[((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (this->divb_mul_)] = -1; + } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -233,8 +240,7 @@ pcl::VoxelGridOcclusionEstimation::rayBoxIntersection (const Eigen::Vect tmin = tzmin; if (tzmax < tmax) tmax = tzmax; - - return tmin; + return std::max(tmin, 0.0f); // We only want to go in "positive" direction here, not in negative. This is relevant if the origin is inside the bounding box } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h b/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h index ffa585ee614..a55b84b3f30 100644 --- a/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h +++ b/filters/include/pcl/filters/voxel_grid_occlusion_estimation.h @@ -47,7 +47,15 @@ namespace pcl /** \brief VoxelGrid to estimate occluded space in the scene. * The ray traversal algorithm is implemented by the work of * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing' - * + * Example code: + * \code + * pcl::VoxelGridOcclusionEstimation vg; + * vg.setInputCloud (input_cloud); + * vg.setLeafSize (leaf_x, leaf_y, leaf_z); + * vg.initializeVoxelGrid (); + * std::vector > occluded_voxels; + * vg.occlusionEstimationAll (occluded_voxels); + * \endcode * \author Christian Potthast * \ingroup filters */ From 190ae4120d3a0d1aadc5ca609b5ee6d8a24858b9 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 5 Nov 2023 18:30:13 +0100 Subject: [PATCH 2/2] Fix VoxelGridOcclusionEstimation tool - call Update() on vtk classes, otherwise the shown window is empty - use the filtered cloud instead of whole input cloud, otherwise processing is very slow for larger clouds - use the fixed RenderWindowInteractor --- tools/voxel_grid_occlusion_estimation.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/tools/voxel_grid_occlusion_estimation.cpp b/tools/voxel_grid_occlusion_estimation.cpp index 897c7f20e1f..b2409585b95 100644 --- a/tools/voxel_grid_occlusion_estimation.cpp +++ b/tools/voxel_grid_occlusion_estimation.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,7 @@ getCuboid (double minX, double maxX, double minY, double maxY, double minZ, doub { vtkSmartPointer < vtkCubeSource > cube = vtkSmartPointer::New (); cube->SetBounds (minX, maxX, minY, maxY, minZ, maxZ); + cube->Update (); return cube->GetOutput (); } @@ -99,6 +101,7 @@ getVoxelActors (pcl::PointCloud& voxelCenters, treeWireframe->AddInputData (getCuboid (x - s, x + s, y - s, y + s, z - s, z + s)); } + treeWireframe->Update (); vtkSmartPointer < vtkLODActor > treeActor = vtkSmartPointer::New (); @@ -119,6 +122,7 @@ displayBoundingBox (Eigen::Vector3f& min_b, Eigen::Vector3f& max_b, { vtkSmartPointer < vtkAppendPolyData > treeWireframe = vtkSmartPointer::New (); treeWireframe->AddInputData (getCuboid (min_b[0], max_b[0], min_b[1], max_b[1], min_b[2], max_b[2])); + treeWireframe->Update(); vtkSmartPointer < vtkActor > treeActor = vtkSmartPointer::New (); @@ -218,17 +222,19 @@ int main (int argc, char** argv) (*occ_centroids)[i] = point; } + // we use the filtered cloud because it contains exactly one point per occupied voxel (avoids drawing the same voxel box multiple times) + CloudT filtered_cloud = vg.getFilteredPointCloud(); CloudT::Ptr cloud_centroids (new CloudT); - cloud_centroids->width = input_cloud->size (); + cloud_centroids->width = filtered_cloud.size (); cloud_centroids->height = 1; cloud_centroids->is_dense = false; - cloud_centroids->points.resize (input_cloud->size ()); + cloud_centroids->points.resize (filtered_cloud.size ()); - for (std::size_t i = 0; i < input_cloud->size (); ++i) + for (std::size_t i = 0; i < filtered_cloud.size (); ++i) { - float x = (*input_cloud)[i].x; - float y = (*input_cloud)[i].y; - float z = (*input_cloud)[i].z; + float x = filtered_cloud[i].x; + float y = filtered_cloud[i].y; + float z = filtered_cloud[i].z; Eigen::Vector3i c = vg.getGridCoordinates (x, y, z); Eigen::Vector4f xyz = vg.getCentroidCoordinate (c); PointT point; @@ -254,7 +260,7 @@ int main (int argc, char** argv) vtkSmartPointer::New(); renderWindow->AddRenderer (renderer); vtkSmartPointer renderWindowInteractor = - vtkSmartPointer::New(); + vtkSmartPointer::Take (vtkRenderWindowInteractorFixNew ()); renderWindowInteractor->SetRenderWindow (renderWindow); // Add the actors to the renderer, set the background and size