i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +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
corelib/src/Rtabmap.cpp
+8 -2
View File
@@ -1667,7 +1667,10 @@ bool Rtabmap::process(
Link tmp = signature->getLinks().begin()->second.inverse(); 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 the previous node is an intermediate node, remove it from the local graph
if(_constraints.size() && if(_constraints.size() &&
@@ -1723,7 +1726,10 @@ bool Rtabmap::process(
if(!_odomCachePoses.empty()) if(!_odomCachePoses.empty())
{ {
float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm(); float odomDistance = (_odomCachePoses.rbegin()->second.inverse() * signature->getPose()).getNorm();
_distanceTravelled += odomDistance; if(!smallDisplacement)
{
_distanceTravelled += odomDistance;
}
while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize) while(!_odomCachePoses.empty() && (int)_odomCachePoses.size() > _maxOdomCacheSize)
{ {