i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +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

2
corelib/src/DBDriverSqlite3.cpp
View File

@@ -3726,7 +3726,7 @@ void DBDriverSqlite3::loadLinksQuery(
query << "SELECT to_id, type, transform FROM Link "; query << "SELECT to_id, type, transform FROM Link ";
} }
query << "WHERE from_id = " << signatureId; query << "WHERE from_id = " << signatureId;
if(typeIn != Link::kUndef) if(typeIn < Link::kEnd)
{ {
if(uStrNumCmp(_version, "0.7.4") >= 0) if(uStrNumCmp(_version, "0.7.4") >= 0)
{ {

17
corelib/src/Memory.cpp
View File

@@ -1354,7 +1354,10 @@ std::map<int, int> Memory::getNeighborsId(
) const ) const
{ {
UASSERT(maxGraphDepth >= 0); 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) if(dbAccessTime)
{ {
*dbAccessTime = 0; *dbAccessTime = 0;
@@ -1412,6 +1415,10 @@ std::map<int, int> Memory::getNeighborsId(
UTimer timer; UTimer timer;
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks); _dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
if(tmpLinks.empty())
{
UWARN("No links loaded for %d?!", *jter);
}
if(!ignoreLoopIds) if(!ignoreLoopIds)
{ {
for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();) 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); tmpLandmarks.insert(*kter);
tmpLinks.erase(kter++); tmpLinks.erase(kter++);
} }
else if(kter->second.from() == kter->second.to())
{
// ignore self-referring links
tmpLinks.erase(kter++);
}
else else
{ {
++kter; ++kter;
@@ -1436,7 +1448,8 @@ std::map<int, int> Memory::getNeighborsId(
// links // links
for(std::multimap<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter) 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); UASSERT(iter->second.type() != Link::kUndef);
if(iter->second.type() == Link::kNeighbor || if(iter->second.type() == Link::kNeighbor ||

2
corelib/src/Rtabmap.cpp
View File

@@ -4078,7 +4078,7 @@ void Rtabmap::optimizeCurrentMap(
int * iterationsDone) const int * iterationsDone) const
{ {
//Optimize the map //Optimize the map
UINFO("Optimize map: around location %d", id); UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false");
if(_memory && id > 0) if(_memory && id > 0)
{ {
UTimer timer; UTimer timer;

1
corelib/src/RtabmapThread.cpp
View File

@@ -128,6 +128,7 @@ void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabaseP
void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
{ {
UDEBUG("optimized=%s, full=%s, graphOnly=%s", optimized?"true":"false", full?"true":"false", graphOnly?"true":"false");
if(_rtabmap) if(_rtabmap)
{ {
std::map<int, Signature> signatures; std::map<int, Signature> signatures;