i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing occupancy grids not 3d spam when subscribing octomap and grids are 2d (#1763)

Browse Source
This commit is contained in:
matlabbe
2026-09-09 10:42:46 -07:00
committed by GitHub
parent f7752bab64
commit fb457255b7
2 changed files with 36 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/global_map/OctoMap.cpp
+18 -2
View File
@@ -479,6 +479,11 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
{
float rangeMaxSqrd = rangeMax_*rangeMax_;
float cellSize = octree_->getResolution();
int not3DCount = 0;
int not3DFirstId = 0;
int not3DGroundType = 0;
int not3DObstaclesType = 0;
int not3DEmptyType = 0;
for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
std::map<int, LocalGrid>::const_iterator localGridIter;
@@ -491,8 +496,13 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
if(!localGridIter->second.is3D())
{
UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
ground.type(), obstacles.type(), emptyCells.type());
if(++not3DCount == 1)
{
not3DFirstId = iter->first;
not3DGroundType = ground.type();
not3DObstaclesType = obstacles.type();
not3DEmptyType = emptyCells.type();
}
continue;
}
@@ -762,6 +772,12 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
UDEBUG("Did not find %d in cache", iter->first);
}
}
if(not3DCount)
{
UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! "
"(%d local grid(s) ignored, first one (id=%d) had ground type=%d, obstacles type=%d, empty type=%d)",
not3DCount, not3DFirstId, not3DGroundType, not3DObstaclesType, not3DEmptyType);
}
}
if(emptyFloodFillDepth_>0)