i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added max ground height parameter for MapsManagerand obstacles_detection

Browse Source
This commit is contained in:
Mathieu Labbe
2016-04-15 17:44:36 -04:00
parent d730c60342
commit 7e5c7d7462
3 changed files with 41 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+29 -8
View File
@@ -45,7 +45,9 @@ MapsManager::MapsManager(bool usePublicNamespace) :
scanOutputVoxelized_(false),
projMaxGroundAngle_(45.0), // degrees
projMinClusterSize_(20),
projMaxHeight_(2.0), // meters
projMaxObstaclesHeight_(2.0), // meters (<=0 disabled)
projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true)
projDetectFlatObstacles_(false),
gridCellSize_(0.05), // meters
gridSize_(0), // meters
gridEroded_(false),
@@ -87,7 +89,19 @@ MapsManager::MapsManager(bool usePublicNamespace) :
//projection map stuff
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
pnh.param("proj_max_height", projMaxHeight_, projMaxHeight_);
if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height"))
{
ROS_WARN("Parameter \"proj_max_height\" has been renamed "
"to \"proj_max_obstacles_height\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
}
else
{
pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
}
pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_);
pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_);
// common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
@@ -194,6 +208,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// filter nodes
if(mapFilterRadius_ > 0.0)
{
UDEBUG("Filter nodes...");
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -244,6 +259,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
scanRequired ||
gridRequired)
{
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
if(findIter != signatures.end())
{
@@ -272,6 +288,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
if(rgbDepthRequired)
{
UDEBUG("rgbDepthRequired");
if(!image.empty() && !depth.empty())
{
pcl::IndicesPtr validIndices(new std::vector<int>);
@@ -300,6 +317,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
else if(depthRequired)
{
UDEBUG("depthRequired");
if( !depth.empty())
{
pcl::IndicesPtr validIndices(new std::vector<int>);
@@ -357,13 +375,14 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(depthRequired)
{
UDEBUG("Creating proj map for %d...", iter->first);
cv::Mat ground, obstacles;
if(cloudRGB.get())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
if(cloudClipped->size() && projMaxHeight_ > 0)
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
}
if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_)
{
@@ -376,15 +395,15 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
}
}
else if(cloudXYZ.get())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxHeight_ > 0)
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
{
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
}
if(cloudClipped->size())
{
@@ -393,7 +412,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
UDEBUG("util3d::occupancy2DFromCloud3D()");
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
}
}
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
@@ -458,6 +478,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
// cleanup not used nodes
UDEBUG("Cleanup not used nodes");
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
iter!=clouds_.end();)
{
src/MapsManager.h
+3 -1
View File
@@ -81,7 +81,9 @@ private:
bool scanOutputVoxelized_;
double projMaxGroundAngle_;
int projMinClusterSize_;
double projMaxHeight_;
double projMaxObstaclesHeight_;
double projMaxGroundHeight_;
bool projDetectFlatObstacles_;
double gridCellSize_;
double gridSize_;
bool gridEroded_;
src/nodelets/obstacles_detection.cpp
+9 -3
View File
@@ -72,6 +72,7 @@ public:
clusterRadius_(0.05),
minClusterSize_(20),
maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
maxGroundHeight_(0.0), // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
segmentFlatObstacles_(false),
waitForTransform_(false),
optimizeForCloseObjects_(false)
@@ -105,6 +106,7 @@ private:
}
pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
pnh.param("max_ground_height", maxGroundHeight_, maxGroundHeight_);
pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
@@ -177,7 +179,8 @@ private:
groundNormalAngle_,
clusterRadius_,
minClusterSize_,
segmentFlatObstacles_);
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -211,7 +214,8 @@ private:
groundNormalAngle_,
clusterRadius_,
minClusterSize_,
segmentFlatObstacles_);
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -234,7 +238,8 @@ private:
2.*groundNormalAngle_,
3.*clusterRadius_,
minClusterSize_,
segmentFlatObstacles_);
segmentFlatObstacles_,
maxGroundHeight_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
@@ -286,6 +291,7 @@ private:
double clusterRadius_;
int minClusterSize_;
double maxObstaclesHeight_;
double maxGroundHeight_;
bool segmentFlatObstacles_;
bool waitForTransform_;
bool optimizeForCloseObjects_;