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

Support intermediate nodes with memory management enabled (#1687)

Browse Source 
* Support intermediate nodes with memory management enabled

* Adding fix from #1686

* Fixed odomCache overlay shown in mapping mode

* Fixed local retrieval with intermediate nodes

* bumping version for updating API. Fixed getting graph error.

* Disable planning if intermediate nodes are there. Updated usage of Kp/BadSignRatio to support intermediate nodes.
This commit is contained in:
matlabbe
2026-04-19 16:08:03 -07:00
committed by GitHub
parent cc86af4399
commit 9cbe84e445
15 changed files with 385 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
corelib/src/Memory.cpp
View File

@@ -1434,23 +1434,30 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
{
Signature * s = this->_getSignature(id);
UASSERT(s!=0);
std::multimap<int, Link> links = s->getLinks();
// Setting true to make sure we save all visual
// words that could be referenced in a previously
// transferred node in LTM (#979)
reducedId = reduceNode(s->id(), 0, true);
if(reducedToOut) {
*reducedToOut = reducedId;
}
if(reducedId>0)
if(s->getWeight() == -1)
{
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
UERROR("Graph reduction with intermediate nodes is not supported.");
}
else
{
std::multimap<int, Link> links = s->getLinks();
// Setting true to make sure we save all visual
// words that could be referenced in a previously
// transferred node in LTM (#979)
reducedId = reduceNode(s->id(), 0, true);
if(reducedToOut) {
*reducedToOut = reducedId;
}
if(reducedId>0)
{
if(iter->second.type() == Link::kNeighbor)
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(_lastGlobalLoopClosureId == s->id())
if(iter->second.type() == Link::kNeighbor)
{
_lastGlobalLoopClosureId = iter->first;
if(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = iter->first;
}
}
}
}
@@ -1674,7 +1681,7 @@ std::map<int, int> Memory::getNeighborsId(
) const
{
UASSERT(maxGraphDepth >= 0);
//DEBUG("signatureId=%d maxGraphDepth=%d maxCheckedInDatabase=%d incrementMarginOnLoop=%d "
//UDEBUG("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);
@@ -1711,17 +1718,19 @@ std::map<int, int> Memory::getNeighborsId(
std::map<int, Link> tmpLandmarks;
const std::multimap<int, Link> * links = &tmpLinks;
const std::map<int, Link> * landmarks = &tmpLandmarks;
bool isIntermediateNode = false;
if(s)
{
if(!ignoreIntermediateNodes || s->getWeight() != -1)
isIntermediateNode = s->getWeight() == -1;
if(!ignoreIntermediateNodes || !isIntermediateNode)
{
ids.insert(std::pair<int, int>(*jter, m));
int effectiveMargin = m>0 && isIntermediateNode>0 ? m-1 : m;
ids.insert(std::pair<int, int>(s->id(), effectiveMargin));
}
else
{
ignoredIds.insert(*jter);
ignoredIds.insert(s->id());
}
links = &s->getLinks();
if(!ignoreLoopIds)
{
@@ -1730,8 +1739,22 @@ std::map<int, int> Memory::getNeighborsId(
}
else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 && _dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
{
++nbLoadedFromDb;
ids.insert(std::pair<int, int>(*jter, m));
int weight = 0;
_dbDriver->getWeight(*jter, weight);
isIntermediateNode = weight == -1;
if(!ignoreIntermediateNodes || !isIntermediateNode)
{
int effectiveMargin = m>0 && isIntermediateNode>0 ? m-1 : m;
ids.insert(std::pair<int, int>(*jter, effectiveMargin));
if(!isIntermediateNode)
{
++nbLoadedFromDb;
}
}
else
{
ignoredIds.insert(*jter);
}
UTimer timer;
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
@@ -1775,7 +1798,7 @@ std::map<int, int> Memory::getNeighborsId(
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
if(ignoreIntermediateNodes && s->getWeight()==-1)
if(isIntermediateNode)
{
// stay on the same margin
if(currentMargin.insert(iter->first).second)
@@ -1906,7 +1929,7 @@ int Memory::getNextId()
int Memory::incrementMapId(std::map<int, int> * reducedIds)
{
//don't increment if there is no location in the current map
const Signature * s = getLastWorkingSignature();
const Signature * s = getLastWorkingSignature(false);
if(s && s->mapId() == _idMapCount)
{
// New session! move all signatures from the STM to WM
@@ -2286,12 +2309,30 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
if(signatures.size())
{
Signature * s = dynamic_cast<Signature *>(signatures.front());
Signature * s = dynamic_cast<Signature *>(signatures.front());
if(s)
{
signaturesRemoved.push_back(s->id());
int refId = s->id();
signaturesRemoved.push_back(refId);
std::multimap<int, Link> neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true);
this->moveToTrash(s);
wordsRemoved = _vwd->getUnusedWordsSize();
// Remove all linked intermediate nodes at the same time (in both direction)
std::list<int> idsToCheck(uKeysList(neighborLinks));
while(!idsToCheck.empty())
{
int id = idsToCheck.front();
idsToCheck.pop_front();
s = this->_getSignature(id);
if(s && s->getWeight() == -1)
{
neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true);
uAppend(idsToCheck, uKeysList(neighborLinks));
signaturesRemoved.push_back(s->id());
this->moveToTrash(s);
}
}
}
else
{
@@ -2310,22 +2351,56 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
UDEBUG("");
// Remove one more than total added during the iteration
int signaturesAdded = _signaturesAdded;
std::list<Signature *> signatures = getRemovableSignatures(signaturesAdded+1, ignoredIds);
for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
int intermediateNodesRemoved = 0;
while(int(signaturesRemoved.size()-intermediateNodesRemoved) < signaturesAdded+1)
{
signaturesRemoved.push_back((*iter)->id());
// When a signature is deleted, it notifies the memory
// and it is removed from the memory list
this->moveToTrash(*iter);
std::list<Signature *> signatures = this->getRemovableSignatures(1, ignoredIds);
if(signatures.size())
{
Signature * s = dynamic_cast<Signature *>(signatures.front());
if(s)
{
signaturesRemoved.push_back(s->id());
std::multimap<int, Link> neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true);
// When a signature is deleted, it notifies the memory
// and it is removed from the memory list
this->moveToTrash(s);
// Remove all linked intermediate nodes at the same time (in both direction)
std::list<int> idsToCheck(uKeysList(neighborLinks));
while(!idsToCheck.empty())
{
int id = idsToCheck.front();
idsToCheck.pop_front();
s = this->_getSignature(id);
if(s && s->getWeight() == -1)
{
++intermediateNodesRemoved;
neighborLinks = graph::filterLinks(s->getLinks(), Link::kNeighbor, true);
uAppend(idsToCheck, uKeysList(neighborLinks));
signaturesRemoved.push_back(s->id());
this->moveToTrash(s);
}
}
}
else
{
break;
}
}
else
{
break;
}
}
if((int)signatures.size() < signaturesAdded)
if(int(signaturesRemoved.size() - intermediateNodesRemoved) < signaturesAdded)
{
UWARN("Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
(int)signatures.size(), signaturesAdded);
UWARN("Less signatures transferred (%d, inter=%d) than added (%d)! The working memory cannot decrease in size.",
int(signaturesRemoved.size()-intermediateNodesRemoved), intermediateNodesRemoved, signaturesAdded);
}
else
{
UDEBUG("signaturesRemoved=%d, _signaturesAdded=%d", (int)signatures.size(), signaturesAdded);
UDEBUG("signaturesRemoved=%d (inter=%d), _signaturesAdded=%d", int(signaturesRemoved.size()-intermediateNodesRemoved), intermediateNodesRemoved, signaturesAdded);
}
}
return signaturesRemoved;
@@ -2578,9 +2653,10 @@ std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<
break;
}
}
if(!foundInSTM)
if(!foundInSTM && s->getWeight()>=0)
{
// less weighted signature priority to be transferred
// Less weighted signature priority to be transferred
// Ignore intermediate nodes
weightAgeIdMap.insert(std::make_pair(WeightAgeIdKey(s->getWeight(), _transferSortingByWeightId?0.0:memIter->second, s->id()), s));
}
}
@@ -2794,9 +2870,21 @@ int Memory::getLastSignatureId() const
return _idCount;
}
const Signature * Memory::getLastWorkingSignature() const
const Signature * Memory::getLastWorkingSignature(bool ignoreIntermediateNodes) const
{
UDEBUG("");
if(ignoreIntermediateNodes && _lastSignature && _lastSignature->getWeight()==-1)
{
for(std::map<int, Signature *>::const_reverse_iterator iter=_signatures.rbegin();
iter!=_signatures.rend();
++iter)
{
if(iter->second->getWeight() != -1)
{
return iter->second;
}
}
return 0;
}
return _lastSignature;
}
@@ -4785,6 +4873,11 @@ void Memory::copyData(const Signature * from, Signature * to)
}
to->sensorData().setId(to->id());
if(!from->sensorData().globalDescriptors().empty())
{
to->sensorData().setGlobalDescriptors(from->sensorData().globalDescriptors());
}
to->setPose(from->getPose());
}
else
@@ -4817,6 +4910,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool isIntermediateNode = data.id() < 0;
if(this->getSignatures().empty() && isIntermediateNode)
{
UWARN("Ignoring input data with stamp %s because the first node in memory cannot be an intermediate node.", inputData.stamp());
return 0;
}
// uncompress data if needed
if(!isIntermediateNode)
@@ -5039,7 +5138,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
int treeSize= int(_workingMem.size() + _stMem.size());
int meanWordsPerLocation = _feature2D->getMaxFeatures()>0?_feature2D->getMaxFeatures():0;
if(treeSize > 1)
if(meanWordsPerLocation==0 && treeSize > 1)
{
meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
}
@@ -6767,7 +6866,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
{
if(keys.at(i)>0)
{
if(_vwd->addWordRef(keys.at(i), (*j)->id()))
if(!_vwd->addWordRef(keys.at(i), (*j)->id()))
{
UERROR("Could not add word ref %d to node %d!?", keys.at(i), (*j)->id());
}
@@ -6790,6 +6889,7 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
UDEBUG("");
UTimer timer;
std::list<int> idsToLoad;
std::list<int> idsInQueue;
std::map<int, int>::iterator wmIter;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
@@ -6800,10 +6900,14 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
idsToLoad.push_back(*i);
UINFO("Loading location %d from database...", *i);
}
else if(idsToLoad.size() >= maxLoaded)
{
idsInQueue.push_back(*i);
}
}
}
UDEBUG("idsToLoad = %d", idsToLoad.size());
UDEBUG("idsToLoad = %ld (in queue = %ld)", idsToLoad.size(), idsInQueue.size());
std::list<Signature *> reactivatedSigns;
if(_dbDriver)
@@ -6812,8 +6916,13 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
}
timeDbAccess = timer.getElapsedTime();
std::list<int> idsLoaded;
int intermediateNodesLoaded = 0;
for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
{
if((*i)->getWeight() == -1)
{
++intermediateNodesLoaded;
}
if(!(*i)->getLandmarks().empty())
{
// Update landmark indexes
@@ -6857,10 +6966,23 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
}
this->enableWordsRef(idsLoaded);
UDEBUG("time = %fs", timer.ticks());
return std::set<int>(idsToLoad.begin(), idsToLoad.end());
std::set<int> totalLoaded(idsToLoad.begin(), idsToLoad.end());
// Ignore intermediate nodes in the total count of signatures loaded, keep loading next in queue
if(intermediateNodesLoaded > 0 && (int)idsInQueue.size() >= intermediateNodesLoaded)
{
double queueTimeDbAccess = 0.0;
std::set<int> queueLoaded = reactivateSignatures(idsInQueue, maxLoaded-intermediateNodesLoaded, queueTimeDbAccess);
timeDbAccess += queueTimeDbAccess;
totalLoaded.insert(queueLoaded.begin(), queueLoaded.end());
}
return totalLoaded;
}
// return all non-null poses
// returns all non-null poses and links
// if lookInDatabase is false, intermediate nodes are ignored and new neighbor links between non-intermediate nodes are returned
// return unique links between nodes (for neighbors: old->new, for loops: parent->child)
void Memory::getMetricConstraints(
const std::set<int> & ids,
@@ -6883,6 +7005,13 @@ void Memory::getMetricConstraints(
{
if(uContains(poses, *iter))
{
const Signature * s = lookInDatabase?0:this->getSignature(*iter); // If we look in db, we don't ignore intermediate nodes
if(s && s->getWeight() == -1)
{
poses.erase(*iter);
continue;
}
std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{
@@ -6895,14 +7024,15 @@ void Memory::getMetricConstraints(
(jter->second.type() == Link::kNeighbor ||
jter->second.type() == Link::kNeighborMerged))
{
const Signature * s = this->getSignature(jter->first);
s = this->getSignature(jter->first);
UASSERT(s!=0);
if(s->getWeight() == -1)
{
bool validLink = false;
Link link = jter->second;
while(s && s->getWeight() == -1)
{
// skip to next neighbor, well we assume that bad signatures
// skip to next neighbor, well we assume that intermediate signatures
// are only linked by max 2 neighbor links.
std::multimap<int, Link> n = this->getNeighborLinks(s->id(), false);
UASSERT(n.size() <= 2);
@@ -6915,15 +7045,29 @@ void Memory::getMetricConstraints(
link = link.merge(uter->second, uter->second.type());
poses.erase(s->id());
s = s2;
validLink = s->getWeight() != -1;
}
else
{
validLink = false;
break;
}
}
else
{
validLink = false;
break;
}
}
links.insert(std::make_pair(*iter, link));
if(validLink)
{
links.insert(std::make_pair(*iter, link));
}
else
{
poses.erase(s->id());
}
}
else
{

3
corelib/src/Optimizer.cpp
View File

@@ -240,7 +240,8 @@ void Optimizer::getConnectedGraph(
{
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0);
UASSERT(uContains(posesIn, fromId));
UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d",
posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str());
posesOut.clear();
linksOut.clear();

227
corelib/src/Rtabmap.cpp
View File

@@ -382,7 +382,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
{
cv::Mat cov;
this->optimizeCurrentMap(
!_optimizeFromGraphEnd?_memory->getWorkingMem().lower_bound(1)->first:_memory->getWorkingMem().rbegin()->first,
_memory->getLastWorkingSignature(true)->id(),
false, _optimizedPoses, cov, &_constraints);
}
if(_optimizedPoses.lower_bound(1) != _optimizedPoses.end())
@@ -845,7 +845,7 @@ int Rtabmap::getTotalMemSize() const
{
if(_memory)
{
const Signature * s =_memory->getLastWorkingSignature();
const Signature * s =_memory->getLastWorkingSignature(false);
if(s)
{
return s->id();
@@ -896,11 +896,11 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
if(_memory->getLastWorkingSignature()->id() &&
if(_memory->getLastWorkingSignature(true)->id() &&
_optimizedPoses.empty())
{
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
this->optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints);
}
}
else
@@ -967,9 +967,9 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
{
return _memory->labelSignature(id, label);
}
else if(_memory->isIncremental() && _memory->getLastWorkingSignature())
else if(_memory->isIncremental() && _memory->getLastWorkingSignature(true))
{
return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
return _memory->labelSignature(_memory->getLastWorkingSignature(true)->id(), label);
}
else if(!_memory->isIncremental() && !_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
{
@@ -1003,9 +1003,9 @@ bool Rtabmap::setUserData(int id, const cv::Mat & data)
{
return _memory->setUserData(id, data);
}
else if(_memory->getLastWorkingSignature())
else if(_memory->getLastWorkingSignature(true))
{
return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
return _memory->setUserData(_memory->getLastWorkingSignature(true)->id(), data);
}
else
{
@@ -1049,7 +1049,7 @@ void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, int format)
{
if(_memory && _memory->getLastWorkingSignature())
if(_memory && _memory->getLastWorkingSignature(!global))
{
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
@@ -1057,11 +1057,11 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
if(optimized)
{
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints);
}
else
{
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
@@ -1112,11 +1112,15 @@ void Rtabmap::resetMemory()
if(_memory)
{
if(_memory->isReadOnly())
{
UWARN("Memory is reset but the database won't be cleared because read-only mode is enabled.");
}
_memory->init(_databasePath, true, _parameters, true);
if(_memory->getLastWorkingSignature())
if(_memory->getLastWorkingSignature(true))
{
cv::Mat covariance;
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, _optimizedPoses, covariance, &_constraints);
}
if(_bayesFilter)
{
@@ -1424,9 +1428,9 @@ bool Rtabmap::process(
else if(_memory->isIncremental()) // only in mapping mode
{
// Detect if the odometry is reset. If yes, trigger a new map.
if(_memory->getLastWorkingSignature())
if(_memory->getLastWorkingSignature(false))
{
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
const Transform & lastPose = _memory->getLastWorkingSignature(false)->getPose(); // use raw odometry
// look for identity
if(!lastPose.isIdentity() && odomPose.isIdentity())
@@ -1473,14 +1477,14 @@ bool Rtabmap::process(
}
}
signature = _memory->getLastWorkingSignature();
signature = _memory->getLastWorkingSignature(false);
_currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0;
if(!signature)
{
UFATAL("Not supposed to be here...last signature is null?!?");
}
ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId());
ULOGGER_INFO("Processing signature %d (%f) w=%d map=%d", signature->id(), signature->getStamp(), signature->getWeight(), signature->mapId());
timeMemoryUpdate = timer.ticks();
ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
@@ -1523,8 +1527,8 @@ bool Rtabmap::process(
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
// Check small motion if previous node is not an intermediate node (consecutive intermediate nodes are merged later)
if(s->getWeight() >= 0)
{
t = links.begin()->second.transform();
}
@@ -2232,8 +2236,9 @@ bool Rtabmap::process(
maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
}
// no need to do retrieval or immunization of locations if memory management
// is disabled and all nodes are in WM
if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
// is disabled and all nodes are in WM.
// Also skip memory mangement on intermediate nodes
if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0) && signature->getWeight()>=0)
{
if(retrievalId > 0)
{
@@ -2373,11 +2378,13 @@ bool Rtabmap::process(
// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
//============================================================
std::list<int> retrievalLocalIds;
if(_rgbdSlamMode)
if(_rgbdSlamMode && signature->getWeight()>=0)
{
// Priority on locations on the planned path
if(_path.size())
{
// Note: retrieval on path with intermediate nodes is not supported. Note that the planned path would
// eventually fail anyway because intermediate nodes are not in _optimizedPoses.
updateGoalIndex();
float distanceSoFar = 0.0f;
@@ -2500,11 +2507,13 @@ bool Rtabmap::process(
{
nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
}
UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
UINFO("near nodes=%d, max local immunized=%d (immunized by path so far=%d), ratio=%f WM=%d",
(int)nearNodesByDist.size(),
maxLocalLocationsImmunized,
immunizedLocally,
_localImmunizationRatio,
(int)_memory->getWorkingMem().size());
std::list<int> retrievalLocalIdsIntermediate;
for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
++iter)
@@ -2512,17 +2521,29 @@ bool Rtabmap::process(
const Signature * s = _memory->getSignature(iter->second);
if(s!=0)
{
// If there is a change of direction, better to be retrieving
// ALL nearest signatures than only newest neighbors
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
++jter)
if(s->getWeight() != -1 && retrievalLocalIds.size() < _maxLocalRetrieved)
{
if(_memory->getSignature(jter->first) == 0)
// If there is a change of direction, better to be retrieving
// all nearest signatures than only newest neighbors.
// Use getNeighborsId instead of direct links to support intermediate nodes.
std::map<int, int> ids = _memory->getNeighborsId(s->id(), 2, _maxLocalRetrieved-retrievalLocalIds.size(), true, false, false);
for(std::map<int, int>::const_reverse_iterator jter=ids.rbegin();
jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0);
++jter)
{
UINFO("retrieval of node %d on local map", jter->first);
retrievalLocalIds.push_back(jter->first);
if(_memory->getSignature(jter->first) == 0)
{
if(jter->second == 0)
{
UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id());
retrievalLocalIdsIntermediate.push_back(jter->first);
}
else
{
UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, s->id());
retrievalLocalIds.push_back(jter->first);
}
}
}
}
if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
@@ -2539,20 +2560,29 @@ bool Rtabmap::process(
if(retrievalLocalIds.size() < _maxLocalRetrieved)
{
std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
retrievalLocalIdsSet.insert(retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end());
for(std::list<int>::iterator iter=retrievalLocalIds.begin();
iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
++iter)
{
std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false);
std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - (unsigned int)retrievalLocalIds.size() + 1, true, false, false);
for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
jter!=ids.rend() && (retrievalLocalIds.size() < _maxLocalRetrieved || jter->second == 0);
++jter)
{
if(_memory->getSignature(jter->first) == 0 &&
retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
{
UINFO("retrieval of node %d on local map", jter->first);
retrievalLocalIds.push_back(jter->first);
if(jter->second == 0)
{
UINFO("retrieval of intermediate node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter);
retrievalLocalIdsIntermediate.push_back(jter->first);
}
else
{
UINFO("retrieval of node %d (margin=%d) on local map (from=%d)", jter->first, jter->second, *iter);
retrievalLocalIds.push_back(jter->first);
}
retrievalLocalIdsSet.insert(jter->first);
}
}
@@ -2566,6 +2596,7 @@ bool Rtabmap::process(
}
// insert them first to make sure they are loaded.
reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIdsIntermediate.begin(), retrievalLocalIdsIntermediate.end());
reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
}
}
@@ -3207,6 +3238,8 @@ bool Rtabmap::process(
UDEBUG("Not self ref links: %d", (int)graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size());
if(_rgbdSlamMode
&&
signature->getWeight() != -1 // Ignore graph optimization on intermediate nodes
&&
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
@@ -4292,6 +4325,7 @@ bool Rtabmap::process(
{
_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
}
bool lastSignatureWasIntermediateNode = signature->getWeight() == -1;
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
int signatureRemoved = _memory->cleanup();
@@ -4397,10 +4431,20 @@ bool Rtabmap::process(
//============================================================
double totalTime = timerTotal.ticks();
ULOGGER_INFO("Total time processing = %fs...", totalTime);
if((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed))
if(!lastSignatureWasIntermediateNode && // skip memory management on intermediate nodes
((_maxTimeAllowed != 0 && totalTime*1000>_maxTimeAllowed) ||
(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)))
{
ULOGGER_INFO("Removing old signatures because time limit is reached %f>%f or memory is reached %d>%d...", totalTime*1000, _maxTimeAllowed, _memory->getWorkingMem().size(), _maxMemoryAllowed);
if(_maxTimeAllowed!=0 && totalTime*1000>_maxTimeAllowed)
{
ULOGGER_INFO("Removing old signatures because time limit is reached %f ms > %f ms...",
totalTime*1000, _maxTimeAllowed);
}
if(_maxMemoryAllowed != 0 && _memory->getWorkingMem().size() > _maxMemoryAllowed)
{
ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...",
_memory->getWorkingMem().size(), _maxMemoryAllowed);
}
immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
std::list<int> transferred = _memory->forget(immunizedLocations);
signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
@@ -4448,9 +4492,9 @@ bool Rtabmap::process(
}
else if(_memory->isIncremental() &&
_optimizedPoses.size() &&
_memory->getLastWorkingSignature())
_memory->getLastWorkingSignature(true))
{
id = _memory->getLastWorkingSignature()->id();
id = _memory->getLastWorkingSignature(true)->id();
UDEBUG("Refresh local map from %d", id);
}
UDEBUG("id=%d _optimizedPoses=%d", id, (int)_optimizedPoses.size());
@@ -4870,9 +4914,14 @@ void Rtabmap::setWorkingDirectory(std::string path)
void Rtabmap::rejectLastLoopClosure()
{
if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
if(!_memory)
{
std::multimap<int, Link> links = _memory->getLinks(getLastLocationId(), false);
return;
}
const Signature * lastS = _memory->getLastWorkingSignature(true); // last non-intermediate
if(lastS && _memory->getStMem().find(lastS->id())!=_memory->getStMem().end())
{
std::multimap<int, Link> links = _memory->getLinks(lastS->id(), false);
bool linksRemoved = false;
for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
@@ -4908,7 +4957,7 @@ void Rtabmap::rejectLastLoopClosure()
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> constraints;
cv::Mat covariance;
optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
optimizeCurrentMap(lastS->id(), false, poses, covariance, &constraints);
if(poses.empty())
{
@@ -4919,7 +4968,7 @@ void Rtabmap::rejectLastLoopClosure()
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses;
_constraints = constraints;
_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
_mapCorrection = _optimizedPoses.at(lastS->id()) * lastS->getPose().inverse();
}
}
}
@@ -4931,6 +4980,13 @@ void Rtabmap::deleteLastLocation()
if(_memory && _memory->getStMem().size())
{
int lastId = *_memory->getStMem().rbegin();
const Signature * s = _memory->getSignature(lastId);
UASSERT(s);
if(s->getWeight() == -1)
{
UERROR("Deleting last location with inermediate nodes is not supported. Aborting.");
return;
}
_memory->deleteLocation(lastId);
// we have to re-optimize the graph without the deleted location
if(_memory->isIncremental() && _optimizedPoses.size())
@@ -4959,7 +5015,7 @@ void Rtabmap::deleteLastLocation()
{
std::multimap<int, Link> constraints;
cv::Mat covariance;
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &constraints);
if(poses.empty())
{
@@ -4969,7 +5025,7 @@ void Rtabmap::deleteLastLocation()
{
_optimizedPoses = poses;
_constraints = constraints;
_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id()) * _memory->getLastWorkingSignature(true)->getPose().inverse();
}
}
}
@@ -5211,11 +5267,43 @@ void Rtabmap::optimizeCurrentMap(
UINFO("Optimize map: around location %d (lookInDatabase=%s)", id, lookInDatabase?"true":"false");
if(_memory && id > 0)
{
if(!lookInDatabase && (!_memory->getSignature(id) || _memory->getSignature(id)->getWeight() == -1))
{
UERROR("When doing a local optimization, the root id (%d) must exist and not be an intermediate node! Aborting...", id);
optimizedPoses.clear();
if(constraints)
{
constraints->clear();
}
return;
}
UTimer timer;
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false);
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
id = ids.begin()->first;
if(lookInDatabase)
{
id = ids.begin()->first;
}
else
{
// Find first node that is not intermediate
for(auto pair: ids)
{
// Make sure fromId is not an intermediate node
const Signature * s = _memory->getSignature(pair.first);
if(s && s->getWeight() != -1)
{
id = pair.first;
break;
}
else if(!s)
{
UWARN("Not found node %d in memory?!", pair.first);
}
}
}
}
UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
@@ -5569,7 +5657,7 @@ void Rtabmap::getGraph(
bool withWords,
bool withGlobalDescriptors) const
{
if(_memory && _memory->getLastWorkingSignature())
if(_memory && _memory->getLastWorkingSignature(!global))
{
if(_rgbdSlamMode)
{
@@ -5577,7 +5665,7 @@ void Rtabmap::getGraph(
{
poses = _optimizedPoses; // guess
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
this->optimizeCurrentMap(_memory->getLastWorkingSignature(!global)->id(), global, poses, covariance, &constraints);
if(!global && !_optimizedPoses.empty())
{
// We send directly the already optimized poses if they are set
@@ -5587,14 +5675,14 @@ void Rtabmap::getGraph(
}
else
{
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
}
else
{
// no optimization on appearance-only mode
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature(!global)->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
@@ -6242,7 +6330,7 @@ bool Rtabmap::addLink(const Link & link)
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> links;
cv::Mat covariance;
optimizeCurrentMap(this->getLastLocationId(), false, poses, covariance, &links);
optimizeCurrentMap(_memory->getLastWorkingSignature(true)->id(), false, poses, covariance, &links);
if(poses.find(link.from()) == poses.end())
{
@@ -6664,12 +6752,12 @@ bool Rtabmap::computePath(int targetNode, bool global)
int currentNode = 0;
if(_memory->isIncremental())
{
if(!_memory->getLastWorkingSignature())
if(!_memory->getLastWorkingSignature(true))
{
UWARN("Working memory is empty... cannot compute a path");
return false;
}
currentNode = _memory->getLastWorkingSignature()->id();
currentNode = _memory->getLastWorkingSignature(true)->id();
}
else
{
@@ -6821,12 +6909,12 @@ bool Rtabmap::computePath(const Transform & targetPose, float tolerance)
int currentNode = 0;
if(_memory->isIncremental())
{
if(!_memory->getLastWorkingSignature())
if(!_memory->getLastWorkingSignature(true))
{
UWARN("Working memory is empty... cannot compute a path");
return false;
}
currentNode = _memory->getLastWorkingSignature()->id();
currentNode = _memory->getLastWorkingSignature(true)->id();
}
else
{
@@ -6980,6 +7068,7 @@ void Rtabmap::updateGoalIndex()
if( _memory && _path.size())
{
// remove all previous virtual links
bool hasIntermediateNodes = false;
for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
{
const Signature * s = _memory->getSignature(_path[i].first);
@@ -6987,6 +7076,10 @@ void Rtabmap::updateGoalIndex()
{
_memory->removeVirtualLinks(s->id());
}
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
}
}
// for the current index, only keep the newest virtual link
@@ -7015,7 +7108,7 @@ void Rtabmap::updateGoalIndex()
// Make sure the next signatures on the path are linked together
float distanceSoFar = 0.0f;
for(unsigned int i=_pathCurrentIndex+1;
i<_path.size();
i<_path.size() && !hasIntermediateNodes;
++i)
{
if(i>0)
@@ -7030,6 +7123,11 @@ void Rtabmap::updateGoalIndex()
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
break;
}
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
@@ -7047,18 +7145,25 @@ void Rtabmap::updateGoalIndex()
}
}
if(hasIntermediateNodes)
{
UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!");
this->clearPath(-1);
return;
}
UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
Transform currentPose;
if(_memory->isIncremental())
{
if(_memory->getLastWorkingSignature() == 0 ||
!uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id()))
if(_memory->getLastWorkingSignature(true) == 0 ||
!uContains(_optimizedPoses, _memory->getLastWorkingSignature(true)->id()))
{
UERROR("Last node is null in memory or not in optimized poses. Aborting the plan...");
this->clearPath(-1);
return;
}
currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature()->id());
currentPose = _optimizedPoses.at(_memory->getLastWorkingSignature(true)->id());
}
else
{

13
corelib/src/RtabmapThread.cpp
View File

@@ -47,8 +47,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rate(Parameters::defaultRtabmapDetectionRate()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_frameRateTimer(new UTimer()),
_previousStamp(0.0),
_previousStamp(-1.0),
_rtabmap(rtabmap),
_paused(false),
lastPose_(Transform::getIdentity())
@@ -62,8 +61,6 @@ RtabmapThread::~RtabmapThread()
UEventsManager::removeHandler(this);
close(true);
delete _frameRateTimer;
}
void RtabmapThread::pushNewState(State newState, const RtabmapEventCmd & cmdEvent)
@@ -88,7 +85,7 @@ void RtabmapThread::clearBufferedData()
_newMapEvents.clear();
lastPose_.setIdentity();
covariance_ = cv::Mat();
_previousStamp = 0;
_previousStamp = -1;
}
_dataMutex.unlock();
@@ -500,9 +497,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
bool ignoreFrame = false;
if(_rate>0.0f)
{
if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate))
{
UDEBUG("Ignoring frame %f (previous stamp=%f, period=%f)",
odomEvent.data().stamp(), _previousStamp, 1.0/_rate);
ignoreFrame = true;
}
}
@@ -540,7 +538,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
else if(!ignoreFrame)
{
_frameRateTimer->start();
_previousStamp = odomEvent.data().stamp();
}

12
corelib/src/odometry/OdometryMono.cpp
View File

@@ -239,7 +239,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
{
UDEBUG("");
bool newPtsAdded = false;
const Signature * newS = memory_->getLastWorkingSignature();
const Signature * newS = memory_->getLastWorkingSignature(false);
UDEBUG("newWords=%d", (int)newS->getWords().size());
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > minInliers_)
@@ -646,7 +646,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
info->type = 1;
}
const Signature * refS = memory_->getLastWorkingSignature();
const Signature * refS = memory_->getLastWorkingSignature(false);
std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
@@ -804,10 +804,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(!refWords3.empty())
{
UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3));
}
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel));
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel));
}
}
else
@@ -829,7 +829,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
// generate kpts
if(memory_->update(SensorData(data)))
{
const Signature * s = memory_->getLastWorkingSignature();
const Signature * s = memory_->getLastWorkingSignature(false);
const std::multimap<int, int> & words = s->getWords();
if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
{

4
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -379,8 +379,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d", id1).c_str());
UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d", id2).c_str());
UASSERT_MSG(poses.find(id1)!=poses.end(), uFormat("id1=%d for constraint %d->%d (type=%d)", id1, id1, id2, iter->second.type()).c_str());
UASSERT_MSG(poses.find(id2)!=poses.end(), uFormat("id2=%d for constraint %d->%d (type=%d)", id2, id1, id2, iter->second.type()).c_str());
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)