i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

OctoMap: fixed empty occupancy not correctly updated when ray tracing is done on more than one local map

Browse Source
This commit is contained in:
matlabbe
2022-01-23 16:29:11 -05:00
parent 83d1e27b81
commit e2dc3ac67e
2 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
View File

@@ -166,7 +166,11 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
UDEBUG("");
UDEBUG("Radius filtering (%ld ground %ld obstacles, radius=%f k=%d)",
groundIndices->size(),
obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0),
noiseFilteringRadius_,
noiseFilteringMinNeighbors_);
if(groundIndices->size())
{
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
@@ -179,6 +183,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
{
*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
groundIndices->size(),
obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0));
if(groundIndices->empty() && obstaclesIndices->empty())
{