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

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)
{
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())
{
@@ -1536,7 +1536,7 @@ std::map<int, float> Memory::getNeighborsIdRadius(
nextMargin.insert(signatureId);
int m = 0;
Transform referential = optimizedPoses.at(signatureId);
UASSERT(!referential.isNull());
UASSERT_MSG(!referential.isNull(), uFormat("signatureId=%d", signatureId).c_str());
float radiusSqrd = radius*radius;
while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
{
@@ -2357,7 +2357,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
}
// 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
}
@@ -3660,6 +3660,10 @@ bool Memory::rehearsalMerge(int oldId, int newId)
{
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)
{
@@ -3725,6 +3729,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
}
newS->setWeight(-9);
}
UDEBUG("New weights: %d->%d %d->%d", oldS->id(), oldS->getWeight(), newS->id(), oldS->getWeight());
// remove location
moveToTrash(_idUpdatedToNewOneRehearsal?oldS:newS, _notLinkedNodesKeptInDb);

4
corelib/src/Rtabmap.cpp
View File

@@ -1353,7 +1353,9 @@ bool Rtabmap::process(
}
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