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

Using Dijkstra for global planning for a significative performance boost (no need to optimize the graph before computing the path)

Browse Source
This commit is contained in:
matlabbe
2015-06-21 18:29:12 -04:00
parent bef408d4b9
commit a5efee20bc
11 changed files with 441 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+48
View File
@@ -876,6 +876,54 @@ std::map<int, Link> Memory::getLoopClosureLinks(
return loopClosures;
}
std::map<int, Link> Memory::getLinks(
int signatureId,
bool lookInDatabase) const
{
std::map<int, Link> links;
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
if(s)
{
links = s->getLinks();
}
else if(lookInDatabase && _dbDriver)
{
_dbDriver->loadLinks(signatureId, links, Link::kUndef);
}
else
{
UWARN("Cannot find signature %d in memory", signatureId);
}
return links;
}
std::multimap<int, Link> Memory::getAllLinks(bool lookInDatabase, bool ignoreNullLinks) const
{
std::multimap<int, Link> links;
if(lookInDatabase && _dbDriver)
{
_dbDriver->getAllLinks(links, ignoreNullLinks);
}
for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
links.erase(iter->first);
for(std::multimap<int, Link>::const_iterator jter=iter->second->getLinks().begin();
jter!=iter->second->getLinks().end();
++jter)
{
if(!ignoreNullLinks || jter->second.isValid())
{
links.insert(std::make_pair(iter->first, jter->second));
}
}
}
return links;
}
// return map<Id,Margin>, including signatureId
// maxCheckedInDatabase = -1 means no limit to check in database (default)
// maxCheckedInDatabase = 0 means don't check in database