i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -214,6 +214,7 @@ private:
std::pair<int, float> _highestHypothesis;
double _lastProcessTime;
bool _someNodesHaveBeenTransferred;
float _distanceTravelled;
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.
corelib/include/rtabmap/core/Statistics.h
+1
View File
@@ -88,6 +88,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Rehearsal_merged,);
RTABMAP_STATS(Memory, Local_graph_size,);
RTABMAP_STATS(Memory, Small_movement,);
RTABMAP_STATS(Memory, Distance_travelled, m);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Scan_matching, ms);
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)
{