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

Added util3d::pasthrough returning indices for convenience. segmentObstaclesFromGound(): filtering obstacles under maxGroundHeight if set

Browse Source
This commit is contained in:
matlabbe
2016-05-21 15:54:37 -04:00
parent 904f4bb4d8
commit 7aa9c92971
3 changed files with 62 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -76,7 +76,8 @@ void segmentObstaclesFromGround(
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2]-0.01 && centroid[2] <= max[2]+0.01) // epsilon
if(centroid[2] >= min[2]-0.01 &&
(centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
@@ -108,6 +109,12 @@ void segmentObstaclesFromGround(
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
// If ground height is set, remove obstacles under it
if(maxGroundHeight > 0.0f)
{
otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
}
//Cluster remaining stuff (obstacles)
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,