i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
+1
View File
@@ -87,6 +87,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
indices = util3d::passThrough(cloud, indices, "z", indices = util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(), minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max()); maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
UDEBUG("indices after max obstacles height filtering = %d", (int)indices->size());
} }
if(indices->size()) if(indices->size())
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+6 -2
View File
@@ -83,7 +83,9 @@ void segmentObstaclesFromGround(
normalKSearch, normalKSearch,
viewPoint); 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; int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters( std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
@@ -93,7 +95,7 @@ void segmentObstaclesFromGround(
minClusterSize, minClusterSize,
std::numeric_limits<int>::max(), std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex); &biggestFlatSurfaceIndex);
UDEBUG("clusteredFlatSurfaces=%d", (int)clusteredFlatSurfaces.size());
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface // cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size()) if(clusteredFlatSurfaces.size())
@@ -138,6 +140,8 @@ void segmentObstaclesFromGround(
ground = flatSurfaces; ground = flatSurfaces;
} }
UDEBUG("ground=%d", (int)ground->size());
if(ground->size() != cloud->size()) if(ground->size() != cloud->size())
{ {
// Remove ground // Remove ground