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/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, 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, 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, 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 #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())); 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 #else

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

@@ -141,7 +141,13 @@ void segmentObstaclesFromGround(
if(ground->size() != cloud->size()) if(ground->size() != cloud->size())
{ {
// Remove ground // 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 ground height is set, remove obstacles under it
if(maxGroundHeight > 0.0f) 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; pcl::IndicesPtr groundIndices, obstaclesIndices;
if(normalsSegmentation_) if(indices->size())
{ {
UDEBUG("normalKSearch=%d", normalKSearch_); if(normalsSegmentation_)
UDEBUG("maxGroundAngle=%f", maxGroundAngle_); {
UDEBUG("Cluster radius=%f", cellSize_*2.0f); UDEBUG("normalKSearch=%d", normalKSearch_);
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0); UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0); UDEBUG("Cluster radius=%f", cellSize_*2.0f);
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>( UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
cloud, UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
indices, util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
groundIndices, cloud,
obstaclesIndices, indices,
normalKSearch_, groundIndices,
maxGroundAngle_, obstaclesIndices,
cellSize_*2.0f, normalKSearch_,
minClusterSize_, maxGroundAngle_,
flatObstaclesDetected_, cellSize_*2.0f,
maxGroundHeight_, minClusterSize_,
0, flatObstaclesDetected_,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1)); maxGroundHeight_,
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0)); 0,
//UWARN("Saving ground.pcd and obstacles.pcd"); Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices); UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices); //UWARN("Saving ground.pcd and obstacles.pcd");
} //pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
else //pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
{ }
UDEBUG(""); else
// passthrough filter {
groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_); UDEBUG("");
obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true); // 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()) if(groundIndices->size())
{ {
groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_); pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
} }
if(obstaclesIndices->size()) 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 " UDEBUG("");
"filtering. Occupancy grid of node %d cannot be " if(groundIsObstacle_)
"created.", {
(int)cloud->size(), node.id()); *obstaclesCloud += *groundCloud;
return; 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);
} }
} else
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_)
{ {
*obstaclesCloud += *groundCloud; UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
groundCloud->clear(); // 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_);
} }
} }
} }

9
guilib/src/DatabaseViewer.cpp
View File

@@ -1546,6 +1546,9 @@ void DatabaseViewer::regenerateLocalMaps()
UTimer time; UTimer time;
OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear();
rtabmap::ProgressDialog progressDialog(this); rtabmap::ProgressDialog progressDialog(this);
progressDialog.setMaximumSteps(ids_.size()); progressDialog.setMaximumSteps(ids_.size());
progressDialog.show(); progressDialog.show();
@@ -1571,12 +1574,6 @@ void DatabaseViewer::regenerateLocalMaps()
uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles))); uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint))); uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids_.size()).arg(time.ticks()); msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids_.size()).arg(time.ticks());
if(i==37)
{
pcl::io::savePCDFileBinary("ground.pcd", *util3d::laserScanToPointCloud(ground));
pcl::io::savePCDFileBinary("obstacles.pcd", *util3d::laserScanToPointCloud(obstacles));
UWARN("Saved ground.pcd and obstacles.pcd");
}
} }
progressDialog.appendText(msg); progressDialog.appendText(msg);