i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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 << "WHERE from_id = " << signatureId;
if(typeIn != Link::kUndef)
if(typeIn < Link::kEnd)
{
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
{
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 ||

2
corelib/src/Rtabmap.cpp
View File

@@ -4078,7 +4078,7 @@ void Rtabmap::optimizeCurrentMap(
int * iterationsDone) const
{
//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)
{
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
{
UDEBUG("optimized=%s, full=%s, graphOnly=%s", optimized?"true":"false", full?"true":"false", graphOnly?"true":"false");
if(_rtabmap)
{
std::map<int, Signature> signatures;