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

Memory::getNeighborsId() fixed no link returned when looking in database

Browse Source
This commit is contained in:
matlabbe
2020-05-30 09:20:32 -04:00
parent 7eae992d6f
commit 6e55525a7b
4 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+15 -2
View File
@@ -1354,7 +1354,10 @@ std::map<int, int> Memory::getNeighborsId(
) const
{
UASSERT(maxGraphDepth >= 0);
//UDEBUG("signatureId=%d, neighborsMargin=%d", signatureId, margin);
//DEBUG("signatureId=%d maxGraphDepth=%d maxCheckedInDatabase=%d incrementMarginOnLoop=%d "
// "ignoreLoopIds=%d ignoreIntermediateNodes=%d ignoreLocalSpaceLoopIds=%d",
// signatureId, maxGraphDepth, maxCheckedInDatabase, incrementMarginOnLoop?1:0,
// ignoreLoopIds?1:0, ignoreIntermediateNodes?1:0, ignoreLocalSpaceLoopIds?1:0);
if(dbAccessTime)
{
*dbAccessTime = 0;
@@ -1412,6 +1415,10 @@ std::map<int, int> Memory::getNeighborsId(
UTimer timer;
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
if(tmpLinks.empty())
{
UWARN("No links loaded for %d?!", *jter);
}
if(!ignoreLoopIds)
{
for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
@@ -1421,6 +1428,11 @@ std::map<int, int> Memory::getNeighborsId(
tmpLandmarks.insert(*kter);
tmpLinks.erase(kter++);
}
else if(kter->second.from() == kter->second.to())
{
// ignore self-referring links
tmpLinks.erase(kter++);
}
else
{
++kter;
@@ -1436,7 +1448,8 @@ std::map<int, int> Memory::getNeighborsId(
// links
for(std::multimap<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
{
if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
if(!uContains(ids, iter->first) &&
ignoredIds.find(iter->first) == ignoredIds.end())
{
UASSERT(iter->second.type() != Link::kUndef);
if(iter->second.type() == Link::kNeighbor ||