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

Fixed saved grid map wrongly cleared when RGBD/OptimizeFromGraphEnd is true

Browse Source
This commit is contained in:
matlabbe
2018-08-02 17:07:48 -04:00
parent bc4b7a860e
commit a97be68897
3 changed files with 47 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MapsManager.h
+1 -1
View File
@@ -52,7 +52,7 @@ public:
bool hasSubscribers() const; bool hasSubscribers() const;
void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const; void backwardCompatibilityParameters(ros::NodeHandle & pnh, rtabmap::ParametersMap & parameters) const;
void setParameters(const rtabmap::ParametersMap & parameters); void setParameters(const rtabmap::ParametersMap & parameters);
void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses); void set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses, const rtabmap::Memory * memory = 0);
std::map<int, rtabmap::Transform> getFilteredPoses( std::map<int, rtabmap::Transform> getFilteredPoses(
const std::map<int, rtabmap::Transform> & poses); const std::map<int, rtabmap::Transform> & poses);
src/CoreWrapper.cpp
+2 -2
View File
@@ -502,8 +502,8 @@ void CoreWrapper::onInit()
cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize); cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
if(!map.empty()) if(!map.empty())
{ {
NODELET_INFO("rtabmap: 2D occupancy grid map loaded.\n"); NODELET_INFO("rtabmap: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses()); mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
} }
} }
src/MapsManager.cpp
+44 -1
View File
@@ -289,9 +289,52 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
#endif #endif
} }
void MapsManager::set2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, rtabmap::Transform> & poses) void MapsManager::set2DMap(
const cv::Mat & map,
float xMin,
float yMin,
float cellSize,
const std::map<int, rtabmap::Transform> & poses,
const rtabmap::Memory * memory)
{ {
occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses); occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
//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)
{
std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
if(!uContains(gridMaps_, iter->first))
{
rtabmap::SensorData data;
data = memory->getSignatureDataConst(iter->first, false, false, false, true);
if(data.gridCellSize() == 0.0f)
{
ROS_WARN("Local occupancy grid doesn't exist for node %d", iter->first);
}
else
{
cv::Mat ground, obstacles, emptyCells;
data.uncompressData(
0,
0,
0,
0,
&ground,
&obstacles,
&emptyCells);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint()));
occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells);
}
}
else
{
occupancyGrid_->addToCache(iter->first, jter->second.first.first, jter->second.first.second, jter->second.second);
}
}
}
} }
void MapsManager::clear() void MapsManager::clear()