i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 01:07:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Memory/Distance_travelled/m to Statistics

Browse Source
This commit is contained in:
matlabbe
2015-10-13 16:57:02 -04:00
parent 88a3f425c1
commit 5f488ed269
4 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+7
View File
@@ -119,6 +119,7 @@ Rtabmap::Rtabmap() :
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
_someNodesHaveBeenTransferred(false),
_distanceTravelled(0.0f),
_epipolarGeometry(0),
_bayesFilter(0),
_graphOptimizer(0),
@@ -335,6 +336,7 @@ void Rtabmap::close()
_mapCorrection.setIdentity();
_lastLocalizationPose.setNull();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
this->clearPath(0);
flushStatisticLogs();
@@ -661,6 +663,7 @@ int Rtabmap::triggerNewMap()
_optimizedPoses.clear();
_constraints.clear();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
}
return mapId;
}
@@ -851,6 +854,7 @@ void Rtabmap::resetMemory()
_mapCorrection.setIdentity();
_lastLocalizationPose.setNull();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
this->clearPath(0);
if(_memory)
@@ -1155,6 +1159,8 @@ bool Rtabmap::process(
Link tmp = signature->getLinks().begin()->second.inverse();
_distanceTravelled += tmp.transform().getNorm();
// if the previous node is an intermediate node, remove it from the local graph
if(_constraints.size() &&
_constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
@@ -2371,6 +2377,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
if(_publishLikelihood || _publishPdf)
{