i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Force full global occupancy grid udpate when the graph has changed (e.g., localizing in other map not connected to current one in database)

Browse Source
This commit is contained in:
matlabbe
2017-04-22 18:43:22 -04:00
parent 183c6abc98
commit af576ea0c4
2 changed files with 34 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
corelib/src/OccupancyGrid.cpp
View File

@@ -422,20 +422,23 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
std::map<int, cv::Mat> emptyLocalMaps; std::map<int, cv::Mat> emptyLocalMaps;
std::map<int, cv::Mat> occupiedLocalMaps; std::map<int, cv::Mat> occupiedLocalMaps;
// First, check of the graph has changed. If so, re-create the map by moving all occupied nodes. // First, check of the graph has changed. If so, re-create the map by moving all occupied nodes (fullUpdate==false).
bool graphChanged = false; bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = true; // If the new map doesn't have any node from the previous map
std::map<int, Transform> transforms; std::map<int, Transform> transforms;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
{ {
std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first); std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
if(jter != posesIn.end()) if(jter != posesIn.end())
{ {
graphChanged = false;
UASSERT(!iter->second.isNull() && !jter->second.isNull()); UASSERT(!iter->second.isNull() && !jter->second.isNull());
Transform t = Transform::getIdentity(); Transform t = Transform::getIdentity();
if(iter->second.getDistanceSquared(jter->second) > 0.0001) if(iter->second.getDistanceSquared(jter->second) > 0.0001)
{ {
t = jter->second * iter->second.inverse(); t = jter->second * iter->second.inverse();
graphChanged = true; graphOptimized = true;
} }
transforms.insert(std::make_pair(jter->first, t)); transforms.insert(std::make_pair(jter->first, t));
@@ -466,10 +469,18 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
} }
} }
if(graphChanged && !map_.empty()) if(graphOptimized || graphChanged)
{ {
UINFO("Graph changed!"); if(graphChanged)
if(!fullUpdate_) {
UWARN("Graph has changed! The whole map should be rebuilt.");
}
else
{
UINFO("Graph optimized!");
}
if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
{ {
// 1) recreate all local maps // 1) recreate all local maps
UASSERT(map_.cols == mapInfo_.cols && UASSERT(map_.cols == mapInfo_.cols &&
@@ -558,7 +569,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
undefinedSize = false; undefinedSize = false;
} }
bool incrementalGraphUpdate = graphChanged && !fullUpdate_; bool incrementalGraphUpdate = graphOptimized && !fullUpdate_ && !graphChanged;
std::list<std::pair<int, Transform> > poses; std::list<std::pair<int, Transform> > poses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0; int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;

22
corelib/src/OctoMap.cpp
View File

@@ -88,7 +88,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size()); UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes. // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
bool graphChanged = false; bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
bool graphChanged = true; // If the new map doesn't have any node from the previous map
std::map<int, Transform> transforms; std::map<int, Transform> transforms;
std::map<int, Transform> updatedAddedNodes; std::map<int, Transform> updatedAddedNodes;
for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter) for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
@@ -96,25 +97,34 @@ void OctoMap::update(const std::map<int, Transform> & poses)
std::map<int, Transform>::const_iterator jter = poses.find(iter->first); std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
if(jter != poses.end()) if(jter != poses.end())
{ {
graphChanged = false;
UASSERT(!iter->second.isNull() && !jter->second.isNull()); UASSERT(!iter->second.isNull() && !jter->second.isNull());
Transform t = Transform::getIdentity(); Transform t = Transform::getIdentity();
if(iter->second.getDistanceSquared(jter->second) > 0.0001) if(iter->second.getDistanceSquared(jter->second) > 0.0001)
{ {
t = jter->second * iter->second.inverse(); t = jter->second * iter->second.inverse();
graphChanged = true; graphOptimized = true;
} }
transforms.insert(std::make_pair(jter->first, t)); transforms.insert(std::make_pair(jter->first, t));
updatedAddedNodes.insert(std::make_pair(jter->first, jter->second)); updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
} }
else else
{ {
UWARN("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first); UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
} }
} }
if(graphChanged) if(graphOptimized || graphChanged)
{ {
UINFO("Graph changed!"); if(graphChanged)
if(fullUpdate_) {
UWARN("Graph has changed! The whole map should be rebuilt.");
}
else
{
UINFO("Graph optimized!");
}
if(fullUpdate_ || graphChanged)
{ {
// clear all but keep cache // clear all but keep cache
octree_->clear(); octree_->clear();