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

updated local loop closure (space) map transform on localization

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@2054 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-20 16:47:18 +00:00
parent 57dec696cf
commit 6bf271e04d
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

@@ -1399,9 +1399,9 @@ bool Rtabmap::process(const SensorData & data)
_memory->addLoopClosureLink(localSpaceNearestId, signature->id(), t, false);
// Old map -> new map, used for localization correction on loop closure
//const Signature * oldS = _memory->getSignature(localSpaceNearestId);
//UASSERT(oldS != 0);
_mapTransform = poses.at(localSpaceNearestId) * t.inverse() * poses.at(signature->id()).inverse();
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
UASSERT(oldS != 0);
_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
}
else
{