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

Statistics: Added Odometry_correction_acc_*

Browse Source
This commit is contained in:
matlabbe
2020-06-18 14:47:00 -04:00
parent a2b0de2a4d
commit 554b08256b
3 changed files with 43 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -339,6 +339,7 @@ private:
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
std::vector<float> _odomCorrectionAcc;
// Planning stuff // Planning stuff
int _pathStatus; int _pathStatus;

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

@@ -52,6 +52,7 @@ namespace rtabmap {
class RTABMAP_EXP Statistics class RTABMAP_EXP Statistics
{ {
RTABMAP_STATS(Loop, Id,); // Combined loop or proximity detection
RTABMAP_STATS(Loop, RejectedHypothesis,); RTABMAP_STATS(Loop, RejectedHypothesis,);
RTABMAP_STATS(Loop, Accepted_hypothesis_id,); RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
RTABMAP_STATS(Loop, Suppressed_hypothesis_id,); RTABMAP_STATS(Loop, Suppressed_hypothesis_id,);
@@ -85,6 +86,15 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Odom_correction_roll, deg); RTABMAP_STATS(Loop, Odom_correction_roll, deg);
RTABMAP_STATS(Loop, Odom_correction_pitch, deg); RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
RTABMAP_STATS(Loop, Odom_correction_yaw, deg); RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
//Odom correction
RTABMAP_STATS(Loop, Odom_correction_acc_norm, m);
RTABMAP_STATS(Loop, Odom_correction_acc_angle, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_x, m);
RTABMAP_STATS(Loop, Odom_correction_acc_y, m);
RTABMAP_STATS(Loop, Odom_correction_acc_z, m);
RTABMAP_STATS(Loop, Odom_correction_acc_roll, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_pitch, deg);
RTABMAP_STATS(Loop, Odom_correction_acc_yaw, deg);
// Map to Odom // Map to Odom
RTABMAP_STATS(Loop, MapToOdom_norm, m); RTABMAP_STATS(Loop, MapToOdom_norm, m);
RTABMAP_STATS(Loop, MapToOdom_angle, deg); RTABMAP_STATS(Loop, MapToOdom_angle, deg);

33
corelib/src/Rtabmap.cpp
View File

@@ -145,6 +145,7 @@ Rtabmap::Rtabmap() :
_mapCorrection(Transform::getIdentity()), _mapCorrection(Transform::getIdentity()),
_lastLocalizationNodeId(0), _lastLocalizationNodeId(0),
_currentSessionHasGPS(false), _currentSessionHasGPS(false),
_odomCorrectionAcc(6,0),
_pathStatus(0), _pathStatus(0),
_pathCurrentIndex(0), _pathCurrentIndex(0),
_pathGoalIndex(0), _pathGoalIndex(0),
@@ -390,6 +391,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f; _distanceTravelled = 0.0f;
_optimizeFromGraphEndChanged = false; _optimizeFromGraphEndChanged = false;
this->clearPath(0); this->clearPath(0);
@@ -722,6 +724,7 @@ void Rtabmap::setInitialPose(const Transform & initialPose)
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_mapCorrection.setIdentity(); _mapCorrection.setIdentity();
_mapCorrectionBackup.setNull(); _mapCorrectionBackup.setNull();
@@ -747,6 +750,7 @@ int Rtabmap::triggerNewMap()
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
if(!_memory->isIncremental()) if(!_memory->isIncremental())
{ {
@@ -911,6 +915,7 @@ void Rtabmap::resetMemory()
_lastLocalizationNodeId = 0; _lastLocalizationNodeId = 0;
_odomCachePoses.clear(); _odomCachePoses.clear();
_odomCacheConstraints.clear(); _odomCacheConstraints.clear();
_odomCorrectionAcc = std::vector<float>(6,0);
_distanceTravelled = 0.0f; _distanceTravelled = 0.0f;
_optimizeFromGraphEndChanged = false; _optimizeFromGraphEndChanged = false;
this->clearPath(0); this->clearPath(0);
@@ -3154,6 +3159,9 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId); statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
statistics_.setProximityDetectionId(lastProximitySpaceClosureId); statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId)); statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
statistics_.addStatistic(Statistics::kLoopId(), _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId);
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId) if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{ {
@@ -3199,6 +3207,30 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
_odomCorrectionAcc[0]+=x;
_odomCorrectionAcc[1]+=y;
_odomCorrectionAcc[2]+=z;
_odomCorrectionAcc[3]+=roll;
_odomCorrectionAcc[4]+=pitch;
_odomCorrectionAcc[5]+=yaw;
Transform odomCorrectionAcc(
_odomCorrectionAcc[0],
_odomCorrectionAcc[1],
_odomCorrectionAcc[2],
_odomCorrectionAcc[3],
_odomCorrectionAcc[4],
_odomCorrectionAcc[5]);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_norm(), odomCorrectionAcc.getNorm());
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_angle(), odomCorrectionAcc.getAngle()*180.0f/M_PI);
odomCorrectionAcc.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_x(), x);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_y(), y);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_z(), z);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOdom_correction_acc_yaw(), yaw*180.0f/M_PI);
} }
} }
if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity()) if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
@@ -3216,7 +3248,6 @@ bool Rtabmap::process(
statistics_.setMapCorrection(_mapCorrection); statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
statistics_.setLocalizationCovariance(localizationCovariance); statistics_.setLocalizationCovariance(localizationCovariance);
statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
// timings... // timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);