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

rtabmap 0.12.15 required, updated Grid parameters (removed grid_cell_size #161)

Browse Source
This commit is contained in:
matlabbe
2017-04-10 17:17:48 -04:00
parent cd656d5a93
commit 3d5b86500b
3 changed files with 38 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+37 -53
View File
@@ -61,11 +61,6 @@ MapsManager::MapsManager() :
cloudOutputVoxelized_(true),
cloudSubtractFiltering_(false),
cloudSubtractFilteringMinNeighbors_(2),
gridCellSize_(0.05), // meters
gridIncremental_(false),
gridSize_(0), // meters
gridEroded_(false),
footprintRadius_(0.0),
mapFilterRadius_(0.0),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
@@ -82,19 +77,6 @@ MapsManager::MapsManager() :
void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace)
{
// common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
if(gridCellSize_ <= 0)
{
ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_);
}
occupancyGrid_->setCellSize(gridCellSize_);
pnh.param("grid_incremental", gridIncremental_, gridIncremental_); // m
pnh.param("grid_size", gridSize_, gridSize_); // m
pnh.param("grid_eroded", gridEroded_, gridEroded_);
pnh.param("grid_footprint_radius", footprintRadius_, footprintRadius_);
// common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
@@ -115,11 +97,6 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_);
pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_);
ROS_INFO("%s(maps): grid_cell_size = %f", name.c_str(), gridCellSize_);
ROS_INFO("%s(maps): grid_incremental = %s", name.c_str(), gridIncremental_?"true":"false");
ROS_INFO("%s(maps): grid_size = %f", name.c_str(), gridSize_);
ROS_INFO("%s(maps): grid_eroded = %s", name.c_str(), gridEroded_?"true":"false");
ROS_INFO("%s(maps): grid_footprint_radius = %f", name.c_str(), footprintRadius_);
ROS_INFO("%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
ROS_INFO("%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
@@ -133,7 +110,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
#ifdef RTABMAP_OCTOMAP
pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_);
UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0);
octomap_ = new OctoMap(gridCellSize_, octomapOccupancyThr_);
octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
if(octomapTreeDepth_ > 16)
{
@@ -232,6 +209,17 @@ void parameterMoved(
nh.getParam(rosName, v);
parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
}
else if(type.compare("bool"))
{
bool v = uStr2Bool(iter->second);
nh.getParam(rosName, v);
if(rosName.compare("grid_incremental") == 0)
{
v = !v; // new parameter is called kGridGlobalFullUpdate(), which is the inverse
}
parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
}
else
{
ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
@@ -274,6 +262,11 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters);
parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters);
parameterMoved(pnh, "grid_unknown_space_filled_max_range", Parameters::kGridScan2dMaxFilledRange(), parameters);
parameterMoved(pnh, "grid_cell_size", Parameters::kGridCellSize(), parameters);
parameterMoved(pnh, "grid_incremental", Parameters::kGridGlobalFullUpdate(), parameters);
parameterMoved(pnh, "grid_size", Parameters::kGridGlobalMinSize(), parameters);
parameterMoved(pnh, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
parameterMoved(pnh, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
@@ -285,11 +278,18 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
{
parameters_ = parameters;
// don't use grid cell size from parameters as we use grid_cell_size ros param
uInsert(parameters_, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(gridCellSize_)));
occupancyGrid_->parseParameters(parameters_);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
#endif
#endif
}
void MapsManager::clear()
@@ -868,7 +868,7 @@ void MapsManager::publishMaps(
{
if(assembledGroundIndex_.indexedFeatures())
{
subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_);
subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
}
if(subtractedCloud->size())
{
@@ -909,7 +909,7 @@ void MapsManager::publishMaps(
{
if(assembledObstacleIndex_.indexedFeatures())
{
subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_);
subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
}
if(subtractedCloud->size())
{
@@ -944,14 +944,14 @@ void MapsManager::publishMaps(
if(cloudOutputVoxelized_)
{
UASSERT(gridCellSize_ > 0.0);
UASSERT(occupancyGrid_->getCellSize() > 0.0);
if(countGrounds && assembledGround_->size())
{
assembledGround_ = util3d::voxelize(assembledGround_, gridCellSize_);
assembledGround_ = util3d::voxelize(assembledGround_, occupancyGrid_->getCellSize());
}
if(countObstacles && assembledObstacles_->size())
{
assembledObstacles_ = util3d::voxelize(assembledObstacles_, gridCellSize_);
assembledObstacles_ = util3d::voxelize(assembledObstacles_, occupancyGrid_->getCellSize());
}
}
@@ -1058,7 +1058,7 @@ void MapsManager::publishMaps(
{
// create the projection map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_);
cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, occupancyGrid_->getMinMapSize(), octomapTreeDepth_);
if(!pixels.empty())
{
@@ -1178,24 +1178,8 @@ cv::Mat MapsManager::generateGridMap(
float & yMin,
float & gridCellSize)
{
gridCellSize = gridCellSize_;
cv::Mat map;
if(gridIncremental_)
{
occupancyGrid_->update(poses, gridSize_, footprintRadius_);
map = occupancyGrid_->getMap(xMin, yMin);
}
else
{
map = util3d::create2DMapFromOccupancyLocalMaps(
poses,
gridMaps_,
gridCellSize_,
xMin, yMin,
gridSize_,
gridEroded_,
footprintRadius_);
}
return map;
gridCellSize = occupancyGrid_->getCellSize();
occupancyGrid_->update(poses);
return occupancyGrid_->getMap(xMin, yMin);
}