i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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) // Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0) 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()) if(groundIndices->size())
{ {
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_); 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_); *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()) if(groundIndices->empty() && obstaclesIndices->empty())
{ {

6
corelib/src/OctoMap.cpp
View File

@@ -891,7 +891,7 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, orderedPoses.size() == 1); RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, true);
if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown) if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
{ {
n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty); n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
@@ -957,6 +957,10 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
} }
} }
} }
}
if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
{
octree_->updateInnerOccupancy(); octree_->updateInnerOccupancy();
} }