i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 05:50:21 +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/GridMap.cpp
+18 -2
View File
@@ -237,6 +237,11 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
if(!cache().empty())
{
UDEBUG("Updating from cache");
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)
{
if(uContains(cache(), iter->first))
@@ -245,8 +250,13 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
if(!localGrid.is3D())
{
UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
if(++not3DCount == 1)
{
not3DFirstId = iter->first;
not3DGroundType = localGrid.groundCells.type();
not3DObstaclesType = localGrid.obstacleCells.type();
not3DEmptyType = localGrid.emptyCells.type();
}
continue;
}
@@ -339,6 +349,12 @@ void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
uInsert(occupiedLocalMaps, std::make_pair(iter->first, occupied));
}
}
if(not3DCount)
{
UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! "
"(%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(minX != maxX && minY != maxY)
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)