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

Grid: support negative ground/obstacle height thresholds

Browse Source
This commit is contained in:
matlabbe
2017-06-12 11:48:13 -04:00
parent 9554597948
commit bbaac8b2b1
4 changed files with 29 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -82,7 +82,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
}
// filter ground/obstacles zone
if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
if(minGroundHeight_ != 0.0f || maxObstacleHeight_ != 0.0f)
{
indices = util3d::passThrough(cloud, indices, "z",
minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
@@ -98,7 +98,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", clusterRadius_);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
@@ -121,8 +121,15 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
{
UDEBUG("");
// passthrough filter
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
pcl::IndicesPtr notObstacles = groundIndices;
if(indices->size())
{
notObstacles = util3d::extractIndices(cloud, indices, true);
notObstacles = util3d::concatenate(notObstacles, groundIndices);
}
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
}
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());

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

@@ -104,7 +104,7 @@ void segmentObstaclesFromGround(
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
if(maxGroundHeight == 0.0f || min[2] < maxGroundHeight)
{
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
@@ -113,7 +113,7 @@ void segmentObstaclesFromGround(
Eigen::Vector4f centroid(0,0,0,1);
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2]-0.01 &&
(centroid[2] <= max[2]+0.01 || (maxGroundHeight>0 && centroid[2] <= maxGroundHeight+0.01))) // epsilon
(centroid[2] <= max[2]+0.01 || (maxGroundHeight!=0.0f && centroid[2] <= maxGroundHeight+0.01))) // epsilon
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
@@ -154,7 +154,7 @@ void segmentObstaclesFromGround(
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
// If ground height is set, remove obstacles under it
if(maxGroundHeight > 0.0f)
if(maxGroundHeight != 0.0f)
{
otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
}