i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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 // filter ground/obstacles zone
if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f) if(minGroundHeight_ != 0.0f || maxObstacleHeight_ != 0.0f)
{ {
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(),
@@ -98,7 +98,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("maxGroundAngle=%f", maxGroundAngle_); UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", clusterRadius_); UDEBUG("Cluster radius=%f", clusterRadius_);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0); UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0); UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
util3d::segmentObstaclesFromGround<PointT>( util3d::segmentObstaclesFromGround<PointT>(
cloud, cloud,
indices, indices,
@@ -121,8 +121,15 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
{ {
UDEBUG(""); UDEBUG("");
// passthrough filter // passthrough filter
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_); groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
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()); 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; Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), 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) for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{ {
@@ -113,7 +113,7 @@ void segmentObstaclesFromGround(
Eigen::Vector4f centroid(0,0,0,1); Eigen::Vector4f centroid(0,0,0,1);
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid); pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2]-0.01 && 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)); ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
} }
@@ -154,7 +154,7 @@ void segmentObstaclesFromGround(
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true); pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
// If ground height is set, remove obstacles under it // 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()); 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::kGridMaxObstacleHeight(), maxObstacleHeight_);
Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_); Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_); Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
if(maxGroundHeight_ > 0 && if(maxGroundHeight_ != 0.0f &&
maxObstacleHeight_ > 0 && maxObstacleHeight_ != 0.0f &&
maxObstacleHeight_ < maxGroundHeight_) maxObstacleHeight_ < maxGroundHeight_)
{ {
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", 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()); Parameters::kGridMaxObstacleHeight().c_str());
maxObstacleHeight_ = 0; maxObstacleHeight_ = 0;
} }
if(maxGroundHeight_ > 0 && if(maxGroundHeight_ != 0.0f &&
minGroundHeight_ > 0 && minGroundHeight_ != 0.0f &&
maxGroundHeight_ < minGroundHeight_) maxGroundHeight_ < minGroundHeight_)
{ {
UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).", 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).", "segmentation approach. Setting it to cell size (%f).",
Parameters::kGridMaxGroundHeight().c_str(), cellSize_); Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
maxGroundHeight_ = 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->setMinimum(def*1000000.0);
widget->setMaximum(0.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->setValue(value);
widget->setObjectName(key); widget->setObjectName(key);
connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter())); connect(widget, SIGNAL(editingFinished()), this, SLOT(changeParameter()));