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

Stats: distance from last localization not increasing if small motion

Browse Source
This commit is contained in:
matlabbe
2023-08-10 15:17:15 -07:00
parent f0ba56bf5e
commit 6395487b58
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Rtabmap.cpp
View File

@@ -1667,7 +1667,10 @@ bool Rtabmap::process(
Link tmp = signature->getLinks().begin()->second.inverse();
_distanceTravelled += tmp.transform().getNorm();
if(!smallDisplacement)
{
_distanceTravelled += tmp.transform().getNorm();
}
// if the previous node is an intermediate node, remove it from the local graph
if(_constraints.size() &&
@@ -1723,7 +1726,10 @@ bool Rtabmap::process(
if(!_odomCachePoses.empty())
{
float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm();
_distanceTravelled += odomDistance;
if(!smallDisplacement)
{
_distanceTravelled += odomDistance;
}
while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize)
{