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

util3d::segmentObstaclesFromGround() fixed no ground returned if maximum ground height is set and the biggest plane was not under that height (e.g., ceiling having more points than ground)

Browse Source
This commit is contained in:
matlabbe
2020-06-18 21:30:43 -04:00
parent 554b08256b
commit c5ec4f337b
1 changed files with 20 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -97,11 +97,28 @@ void segmentObstaclesFromGround(
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size())
{
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
if(maxGroundHeight != 0.0f)
{
// Search for biggest surface under max ground height
size_t points = 0;
for(size_t i=0;i<clusteredFlatSurfaces.size();++i)
{
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(i), min, max);
if(min[2]<maxGroundHeight && clusteredFlatSurfaces.size() > points)
{
points = clusteredFlatSurfaces.at(i)->size();
biggestFlatSurfaceIndex = i;
}
}
}
else
{
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
}
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
if(!ground->empty() && (maxGroundHeight == 0.0f || min[2] < maxGroundHeight))
{
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{