i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Stats: added Loop/Distance_since_last_loc

Browse Source
This commit is contained in:
matlabbe
2020-09-10 14:07:53 -04:00
parent cabf03af44
commit bd80811ea1
3 changed files with 13 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -310,6 +310,7 @@ private:
double _lastProcessTime; double _lastProcessTime;
bool _someNodesHaveBeenTransferred; bool _someNodesHaveBeenTransferred;
float _distanceTravelled; float _distanceTravelled;
float _distanceTravelledSinceLastLocalization;
bool _optimizeFromGraphEndChanged; bool _optimizeFromGraphEndChanged;
// Abstract classes containing all loop closure // Abstract classes containing all loop closure

1
corelib/include/rtabmap/core/Statistics.h
View File

@@ -66,6 +66,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Visual_words,); RTABMAP_STATS(Loop, Visual_words,);
RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Visual_matches,); RTABMAP_STATS(Loop, Visual_matches,);
RTABMAP_STATS(Loop, Distance_since_last_loc,);
RTABMAP_STATS(Loop, Last_id,); RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(Loop, Optimization_max_error, m); RTABMAP_STATS(Loop, Optimization_max_error, m);
RTABMAP_STATS(Loop, Optimization_max_error_ratio, ); RTABMAP_STATS(Loop, Optimization_max_error_ratio, );

11
corelib/src/Rtabmap.cpp
View File

@@ -134,6 +134,7 @@ Rtabmap::Rtabmap() :
_lastProcessTime(0.0), _lastProcessTime(0.0),
_someNodesHaveBeenTransferred(false), _someNodesHaveBeenTransferred(false),
_distanceTravelled(0.0f), _distanceTravelled(0.0f),
_distanceTravelledSinceLastLocalization(0.0f),
_optimizeFromGraphEndChanged(false), _optimizeFromGraphEndChanged(false),
_epipolarGeometry(0), _epipolarGeometry(0),
_bayesFilter(0), _bayesFilter(0),
@@ -393,6 +394,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0); _odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f; _distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
_optimizeFromGraphEndChanged = false; _optimizeFromGraphEndChanged = false;
this->clearPath(0); this->clearPath(0);
_gpsGeocentricCache.clear(); _gpsGeocentricCache.clear();
@@ -751,6 +753,8 @@ int Rtabmap::triggerNewMap()
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0); _odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
if(!_memory->isIncremental()) if(!_memory->isIncremental())
{ {
@@ -917,6 +921,7 @@ void Rtabmap::resetMemory()
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0); _odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f; _distanceTravelled = 0.0f;
_distanceTravelledSinceLastLocalization = 0.0f;
_optimizeFromGraphEndChanged = false; _optimizeFromGraphEndChanged = false;
this->clearPath(0); this->clearPath(0);
@@ -1380,6 +1385,9 @@ bool Rtabmap::process(
_constraints.insert(std::make_pair(iter->first, iter->second.inverse())); _constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
} }
} }
float distanceTravelledOld = _distanceTravelled;
// only in mapping mode we add a neighbor link // only in mapping mode we add a neighbor link
if(signature->getLinks().size() && if(signature->getLinks().size() &&
signature->getLinks().begin()->second.type() == Link::kNeighbor) signature->getLinks().begin()->second.type() == Link::kNeighbor)
@@ -1452,6 +1460,7 @@ bool Rtabmap::process(
} }
} }
} }
_distanceTravelledSinceLastLocalization += _distanceTravelled - distanceTravelledOld;
//============================================================ //============================================================
// Reduced graph // Reduced graph
@@ -3174,6 +3183,8 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId()); statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId());
statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size()); statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
_distanceTravelledSinceLastLocalization = 0.0f;
// if ground truth exists, compute localization error // if ground truth exists, compute localization error
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull()) if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())