i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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());
}

12
corelib/src/OccupancyGrid.cpp
View File

@@ -98,8 +98,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
if(maxGroundHeight_ > 0 &&
maxObstacleHeight_ > 0 &&
if(maxGroundHeight_ != 0.0f &&
maxObstacleHeight_ != 0.0f &&
maxObstacleHeight_ < maxGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
@@ -108,8 +108,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::kGridMaxObstacleHeight().c_str());
maxObstacleHeight_ = 0;
}
if(maxGroundHeight_ > 0 &&
minGroundHeight_ > 0 &&
if(maxGroundHeight_ != 0.0f &&
minGroundHeight_ != 0.0f &&
maxGroundHeight_ < minGroundHeight_)
{
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
@@ -175,9 +175,9 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
}
}
if(maxGroundHeight_ <= 0.0f && !normalsSegmentation_)
if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
{
UWARN("\"%s\" should be greater than 0 if not using normals "
UWARN("\"%s\" should be not equal to 0 if not using normals "
"segmentation approach. Setting it to cell size (%f).",
Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
maxGroundHeight_ = cellSize_;

9
guilib/src/ParametersToolBox.cpp
View File

@@ -402,6 +402,15 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout,
widget->setMinimum(def*1000000.0);
widget->setMaximum(0.0);
}
// set minimum for selected parameters
if(key.compare(Parameters::kGridMinGroundHeight().c_str()) == 0 ||
key.compare(Parameters::kGridMaxGroundHeight().c_str()) == 0 ||
key.compare(Parameters::kGridMaxObstacleHeight().c_str()) == 0)
{
widget->setMinimum(-1000000.0);
}
widget->setValue(value);
widget->setObjectName(key);
connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));