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

Cleanup gui

Browse Source 
Fixed crash when a new location (B merged with previous location A through rehearsal) and then loop on a location already merged with the location A (which is not anymore in WM)
Updated rejected hypothesis tag

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@299 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-07-23 03:55:05 +00:00
parent 9fe16dd504
commit 649153e1df
9 changed files with 156 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/src/Memory.cpp
View File

@@ -1014,7 +1014,7 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
if(oldS->getLoopClosureId() > 0 && oldS->getLoopClosureId() == newS->id())
{
// do nothing, already merged
newS->setWeight(newS->getWeight() + 1);
UDEBUG("already merged");
}
else if(oldS->getLoopClosureId() > 0)
{
@@ -1023,7 +1023,7 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
}
else if(newS->getLoopClosureId() > 0)
{
ULOGGER_ERROR("Not supposed to occur!");
UFATAL("Not supposed to occur!");
}
else
{
@@ -1040,6 +1040,16 @@ void Memory::addLoopClosureLink(int oldId, int newId, bool rehearsal)
// update weight
newS->setWeight(newS->getWeight() + 1 + oldS->getWeight());
// Find if a location in WM has a loop closure with the old location,
// if yes, redirect the loop closure ref to the new one.
for(std::map<int, Signature *>::iterator iter=_signatures.begin(); iter != _signatures.end(); ++iter)
{
if(_stMem.find(iter->first) == _stMem.end() && iter->second->getLoopClosureId() == oldS->id())
{
iter->second->setLoopClosureId(newS->id());
}
}
}
else
{

7
corelib/src/Rtabmap.cpp
View File

@@ -855,6 +855,10 @@ void Rtabmap::process()
timeHypothesesValidation = timer.ticks();
ULOGGER_INFO("timeHypothesesValidation=%f",timeHypothesesValidation);
}
else if(hypotheses.front().second < _loopRatio*_highestHypothesisValue)
{
rejectedHypothesis = true;
}
//============================================================
// Retrieval id update
@@ -862,6 +866,7 @@ void Rtabmap::process()
std::list<std::pair<int, float> > hyp;
// using likelihood (only if in reactivated ids)
this->selectHypotheses(likelihood, hyp, false);
int lastReactivatedId = _reactivateId;
if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hyp.front().first) != reactivatedIds.end())
{
_reactivateId = hyp.front().first;
@@ -887,7 +892,7 @@ void Rtabmap::process()
}
else if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hypotheses.front().first) != reactivatedIds.end())
{
if(signaturesReactivated<2 || hypotheses.front().second < _loopRatio*_highestHypothesisValue)
if(signaturesReactivated<_maxRetrieved || (hypotheses.front().first == lastReactivatedId && rejectedHypothesis))
{
// We are loosing the next loop closures (it
// can be temporary occlusions/bad images),