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

0.18.3: added apriltags2 support (input topic: tag_detections)

Browse Source
This commit is contained in:
matlabbe
2018-12-07 18:31:59 -05:00
parent 1559d9ba8e
commit 9ad434e27b
14 changed files with 379 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+134 -126
View File
@@ -63,8 +63,8 @@ MapsManager::MapsManager() :
mapFilterRadius_(0.0),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
negativePosesIgnored_(true),
negativeScanEmptyRayTracing_(true),
alwaysUpdateMap_(false),
scanEmptyRayTracing_(true),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
occupancyGrid_(new OccupancyGrid),
@@ -79,8 +79,30 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
pnh.param("map_negative_scan_empty_ray_tracing", negativeScanEmptyRayTracing_, negativeScanEmptyRayTracing_);
if(pnh.hasParam("map_negative_poses_ignored"))
{
ROS_WARN("Parameter \"map_negative_poses_ignored\" has been "
"removed. Use \"map_always_update\" instead.");
if(!pnh.hasParam("map_always_update"))
{
bool negPosesIgnored;
pnh.getParam("map_negative_poses_ignored", negPosesIgnored);
alwaysUpdateMap_ = !negPosesIgnored;
}
}
pnh.param("map_always_update", alwaysUpdateMap_, alwaysUpdateMap_);
if(pnh.hasParam("map_negative_scan_empty_ray_tracing"))
{
ROS_WARN("Parameter \"map_negative_scan_empty_ray_tracing\" has been "
"removed. Use \"map_empty_ray_tracing\" instead.");
if(!pnh.hasParam("map_empty_ray_tracing"))
{
pnh.getParam("map_negative_scan_empty_ray_tracing", scanEmptyRayTracing_);
}
}
pnh.param("map_empty_ray_tracing", scanEmptyRayTracing_, scanEmptyRayTracing_);
if(pnh.hasParam("scan_output_voxelized"))
{
@@ -98,8 +120,8 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
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");
ROS_INFO("%s(maps): map_negative_poses_ignored = %s", name.c_str(), negativePosesIgnored_?"true":"false");
ROS_INFO("%s(maps): map_negative_scan_ray_tracing = %s", name.c_str(), negativeScanEmptyRayTracing_?"true":"false");
ROS_INFO("%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false");
ROS_INFO("%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false");
ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false");
ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
@@ -302,7 +324,7 @@ void MapsManager::set2DMap(
//update cache in case the map should be updated
if(memory)
{
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
if(!uContains(gridMaps_, iter->first))
@@ -388,7 +410,7 @@ std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Trans
}
std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
const std::map<int, rtabmap::Transform> & poses,
const std::map<int, rtabmap::Transform> & posesIn,
const rtabmap::Memory * memory,
bool updateGrid,
bool updateOctomap,
@@ -434,6 +456,16 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
return std::map<int, rtabmap::Transform>();
}
// process only nodes (exclude landmarks)
std::map<int, rtabmap::Transform> poses;
if(posesIn.begin()->first < 0)
{
poses.insert(posesIn.lower_bound(0), posesIn.end());
}
else
{
poses = posesIn;
}
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
@@ -445,17 +477,10 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
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)
if(poses.find(0) != poses.end())
{
if(iter->first <=0)
{
// make sure to keep latest data
filteredPoses.insert(*iter);
}
else
{
break;
}
// make sure to keep latest data
filteredPoses.insert(*poses.find(0));
}
}
else
@@ -463,19 +488,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
filteredPoses = poses;
}
if(negativePosesIgnored_)
if(alwaysUpdateMap_)
{
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
{
if(iter->first <= 0)
{
filteredPoses.erase(iter++);
}
else
{
++iter;
}
}
filteredPoses.erase(0);
}
bool longUpdate = false;
@@ -505,7 +520,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(!iter->second.isNull())
{
rtabmap::SensorData data;
if(updateGridCache && (iter->first < 0 || !uContains(gridMaps_, iter->first)))
if(updateGridCache && (iter->first == 0 || !uContains(gridMaps_, iter->first)))
{
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
@@ -518,78 +533,35 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
}
if(data.id() != 0)
UDEBUG("Adding grid map %d to cache...", iter->first);
cv::Point3f viewPoint;
cv::Mat ground, obstacles, emptyCells;
if(iter->first > 0)
{
UDEBUG("Adding grid map %d to cache...", iter->first);
cv::Point3f viewPoint;
cv::Mat ground, obstacles, emptyCells;
if(iter->first > 0)
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f;
static bool warningShown = false;
if(occupancySavedInDB && generateGrid && !warningShown)
{
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f;
static bool warningShown = false;
if(occupancySavedInDB && generateGrid && !warningShown)
{
warningShown = true;
UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
"any occupancy grid output) but it cannot be found "
"in memory. For convenience, the occupancy "
"grid is regenerated. Make sure parameter \"%s\" is true to "
"avoid this warning for the next locations added to map. For older "
"locations already in database without an occupancy grid map, you can use the "
"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
"save them back in the database for next sessions. This warning is only shown once.",
data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
}
if(memory && occupancySavedInDB && generateGrid)
{
// if we are here, it is because we loaded a database with old nodes not having occupancy grid set
// try reload again
data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
}
data.uncompressData(
occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
0,
generateGrid?0:&ground,
generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
else
{
viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
warningShown = true;
UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
"any occupancy grid output) but it cannot be found "
"in memory. For convenience, the occupancy "
"grid is regenerated. Make sure parameter \"%s\" is true to "
"avoid this warning for the next locations added to map. For older "
"locations already in database without an occupancy grid map, you can use the "
"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
"save them back in the database for next sessions. This warning is only shown once.",
data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
}
else
if(memory && occupancySavedInDB && generateGrid)
{
// generate tmp occupancy grid for negative ids (assuming data is already uncompressed)
// For negative laser scans, fill empty space?
bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(negativeScanEmptyRayTracing_)));
occupancyGrid_->parseParameters(parameters);
}
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_);
data.uncompressData(
// if we are here, it is because we loaded a database with old nodes not having occupancy grid set
// try reload again
data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
}
data.uncompressData(
occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
@@ -598,38 +570,74 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
else
{
viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
// put back
if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
occupancyGrid_->parseParameters(parameters);
}
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
else
{
viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
}
else if(memory)
else
{
ROS_ERROR("Data missing for node %d to update the maps", iter->first);
// generate tmp occupancy grid for latest id (assuming data is already uncompressed)
// For negative laser scans, fill empty space?
bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_)));
occupancyGrid_->parseParameters(parameters);
}
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_);
data.uncompressData(
occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
0,
generateGrid?0:&ground,
generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
else
{
viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
}
// put back
if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
occupancyGrid_->parseParameters(parameters);
}
}
}
if(updateGrid &&
(iter->first < 0 ||
(iter->first == 0 ||
occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
@@ -645,7 +653,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP
if(updateOctomap &&
(iter->first < 0 ||
(iter->first == 0 ||
octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
@@ -811,7 +819,7 @@ void MapsManager::publishMaps(
cloudObstaclesPub_.getNumSubscribers();
bool graphGroundChanged = updateGround;
bool graphObstacleChanged = updateObstacles;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter;
if(updateGround)