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/DetectFlatObstacles default true. segmentObstaclesFromGround(): Fixed not used indices still kept in obstacles output

Browse Source
This commit is contained in:
matlabbe
2016-08-26 11:41:35 -04:00
parent 797f1f0f1c
commit 4e027a6515
4 changed files with 95 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -477,7 +477,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, MaxGroundAngle, float, 45, uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NormalK, int, 10, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
RTABMAP_PARAM(Grid, MinClusterSize, int, 10, uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, false, uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, FlatObstacleDetected, bool, true, uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
#ifdef RTABMAP_OCTOMAP
RTABMAP_PARAM(Grid, 3D, bool, true, uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
#else

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

@@ -141,7 +141,13 @@ void segmentObstaclesFromGround(
if(ground->size() != cloud->size())
{
// Remove ground
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
pcl::IndicesPtr notObstacles = ground;
if(indices->size())
{
notObstacles = util3d::extractIndices(cloud, indices, true);
notObstacles = util3d::concatenate(notObstacles, ground);
}
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
// If ground height is set, remove obstacles under it
if(maxGroundHeight > 0.0f)

165
corelib/src/OccupancyGrid.cpp
View File

@@ -276,101 +276,104 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
pcl::IndicesPtr groundIndices, obstaclesIndices;
if(normalsSegmentation_)
if(indices->size())
{
UDEBUG("normalKSearch=%d", normalKSearch_);
UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", cellSize_*2.0f);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
cloud,
indices,
groundIndices,
obstaclesIndices,
normalKSearch_,
maxGroundAngle_,
cellSize_*2.0f,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,
0,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
}
else
{
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);
if(normalsSegmentation_)
{
UDEBUG("normalKSearch=%d", normalKSearch_);
UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("Cluster radius=%f", cellSize_*2.0f);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
cloud,
indices,
groundIndices,
obstaclesIndices,
normalKSearch_,
maxGroundAngle_,
cellSize_*2.0f,
minClusterSize_,
flatObstaclesDetected_,
maxGroundHeight_,
0,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
}
else
{
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);
}
}
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
UDEBUG("");
if(groundIndices->size())
{
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(obstaclesIndices->size())
{
obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
if(groundIndices->empty() && obstaclesIndices->empty())
{
UWARN("Cloud (with %d points) is empty after noise "
"filtering. Occupancy grid of node %d cannot be "
"created.",
(int)cloud->size(), node.id());
return;
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
UDEBUG("");
if(groundIndices->size())
{
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
}
if(groundIndices->empty() && obstaclesIndices->empty())
if(grid3D_)
{
UWARN("Cloud (with %d points) is empty after noise "
"filtering. Occupancy grid of node %d cannot be "
"created.",
(int)cloud->size(), node.id());
return;
UDEBUG("");
if(groundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
// transform back in base frame
Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
}
if(grid3D_)
{
UDEBUG("");
if(groundIsObstacle_)
else
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
cellSize_);
}
// transform back in base frame
Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
}
else
{
UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
// projection on the xy plane
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
cellSize_);
}
}
}