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

fixed null variance assert on local loop closures in Time/Space

Browse Source
This commit is contained in:
matlabbe
2015-09-09 22:31:02 -04:00
parent 7c85f11fbb
commit 91d87ec6a8
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/Rtabmap.cpp
View File

@@ -1079,7 +1079,7 @@ bool Rtabmap::process(
oldId, oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(), signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str()); t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance, variance); _memory->updateLink(signature->id(), oldId, t, variance>0?variance:0.0001, variance>0?variance:0.0001);
if(_optimizeFromGraphEnd) if(_optimizeFromGraphEnd)
{ {
@@ -1180,7 +1180,7 @@ bool Rtabmap::process(
*iter, *iter,
transform.prettyPrint().c_str()); transform.prettyPrint().c_str());
// Add a loop constraint // Add a loop constraint
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance))) if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001)))
{ {
++localLoopClosuresInTimeFound; ++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s", UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1952,7 +1952,7 @@ bool Rtabmap::process(
signature->id(), signature->id(),
nearestId, nearestId,
transform.prettyPrint().c_str()); transform.prettyPrint().c_str());
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance)); _memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance>0?variance:0.0001, variance>0?variance:0.0001));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId)); loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0) if(_loopClosureHypothesis.first == 0)