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

Fixed weight=-8 bug when moving rehearsed node to trash

Browse Source
This commit is contained in:
matlabbe
2020-11-06 14:47:10 -05:00
parent e7a2f206a0
commit e102243f0e
2 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/Memory.cpp
View File

@@ -1009,7 +1009,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
if(_vwd) if(_vwd)
{ {
UDEBUG("%d words ref for the signature %d", signature->getWords().size(), signature->id()); UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
} }
if(signature->getWords().size()) if(signature->getWords().size())
{ {
@@ -1536,7 +1536,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
nextMargin.insert(signatureId); nextMargin.insert(signatureId);
int m = 0; int m = 0;
Transform referential = optimizedPoses.at(signatureId); Transform referential = optimizedPoses.at(signatureId);
UASSERT(!referential.isNull()); UASSERT_MSG(!referential.isNull(), uFormat("signatureId=%d", signatureId).c_str());
float radiusSqrd = radius*radius; float radiusSqrd = radius*radius;
while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size()) while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
{ {
@@ -2357,7 +2357,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
} }
// child // child
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id()) if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
{ {
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
} }
@@ -3660,6 +3660,10 @@ bool Memory::rehearsalMerge(int oldId, int newId)
{ {
fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull(); fullMerge = newS->hasLink(oldS->id()) && newS->getLinks().begin()->second.transform().isNull();
} }
UDEBUG("fullMerge=%s intermediateMerge=%s _idUpdatedToNewOneRehearsal=%s",
fullMerge?"true":"false",
intermediateMerge?"true":"false",
_idUpdatedToNewOneRehearsal?"true":"false");
if(fullMerge) if(fullMerge)
{ {
@@ -3725,6 +3729,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
} }
newS->setWeight(-9); newS->setWeight(-9);
} }
UDEBUG("New weights: %d->%d %d->%d", oldS->id(), oldS->getWeight(), newS->id(), oldS->getWeight());
// remove location // remove location
moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb); moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);

4
corelib/src/Rtabmap.cpp
View File

@@ -1353,7 +1353,9 @@ bool Rtabmap::process(
} }
else else
{ {
UWARN("Neighbor link refining is activated but there are intermediate nodes, aborting refining..."); UWARN("Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
signature->id(), signature->getWeight(), oldS->id(), oldS->getWeight());
newPose = _mapCorrection * signature->getPose();
} }
} }
else else