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

check if same node before adding virtual link

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-03 14:11:57 -05:00
parent ee885321aa
commit b2e5b7fbfc
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Rtabmap.cpp
View File

@@ -1387,7 +1387,9 @@ bool Rtabmap::process(const SensorData & data)
}
// Add a virtual loop closure link to keep the path linked to local map
if(_path.size() && !signature->hasLink(_path[_pathCurrentIndex]))
if(_path.size() &&
signature->id() != _path[_pathCurrentIndex] &&
!signature->hasLink(_path[_pathCurrentIndex]))
{
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex]);
_memory->addLoopClosureLink(_path[_pathCurrentIndex], signature->id(), virtualLoop, Link::kVirtualClosure, 99999);