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

Fixed bug where Grid/MaxObstacleHeight didn't work when the ground is not observable

Browse Source
This commit is contained in:
matlabbe
2016-11-28 14:02:06 -05:00
parent 2f6d7f81cc
commit 4d585cbb11
2 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -87,6 +87,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
indices = util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
UDEBUG("indices after max obstacles height filtering = %d", (int)indices->size());
}
if(indices->size())

8
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -83,7 +83,9 @@ void segmentObstaclesFromGround(
normalKSearch,
viewPoint);
if(segmentFlatObstacles)
UDEBUG("cloud=%d, indices=%d flatSurfaces=%d", (int)cloud->size(), (int)indices->size(), (int)flatSurfaces->size());
if(segmentFlatObstacles && flatSurfaces->size())
{
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
@@ -93,7 +95,7 @@ void segmentObstaclesFromGround(
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
UDEBUG("clusteredFlatSurfaces=%d", (int)clusteredFlatSurfaces.size());
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size())
@@ -138,6 +140,8 @@ void segmentObstaclesFromGround(
ground = flatSurfaces;
}
UDEBUG("ground=%d", (int)ground->size());
if(ground->size() != cloud->size())
{
// Remove ground