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

Statistics: added MapToOdom and MapToBase stats

Browse Source
This commit is contained in:
matlabbe
2020-05-28 21:04:01 -04:00
parent 45ddce938a
commit 2509b6ee09
2 changed files with 40 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -74,6 +74,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
RTABMAP_STATS(Loop, Visual_inliers_distribution,);
//Odom correction
RTABMAP_STATS(Loop, Odom_correction_norm, m);
RTABMAP_STATS(Loop, Odom_correction_angle, deg);
RTABMAP_STATS(Loop, Odom_correction_x, m);
@@ -82,6 +83,22 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Odom_correction_roll, deg);
RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
// Map to Odom
RTABMAP_STATS(Loop, MapToOdom_norm, m);
RTABMAP_STATS(Loop, MapToOdom_angle, deg);
RTABMAP_STATS(Loop, MapToOdom_x, m);
RTABMAP_STATS(Loop, MapToOdom_y, m);
RTABMAP_STATS(Loop, MapToOdom_z, m);
RTABMAP_STATS(Loop, MapToOdom_roll, deg);
RTABMAP_STATS(Loop, MapToOdom_pitch, deg);
RTABMAP_STATS(Loop, MapToOdom_yaw, deg);
// Map to Base
RTABMAP_STATS(Loop, MapToBase_x, m);
RTABMAP_STATS(Loop, MapToBase_y, m);
RTABMAP_STATS(Loop, MapToBase_z, m);
RTABMAP_STATS(Loop, MapToBase_roll, deg);
RTABMAP_STATS(Loop, MapToBase_pitch, deg);
RTABMAP_STATS(Loop, MapToBase_yaw, deg);
RTABMAP_STATS(Proximity, Time_detections,);
RTABMAP_STATS(Proximity, Space_last_detection_id,);

25
corelib/src/Rtabmap.cpp
View File

@@ -3122,6 +3122,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kProximitySpace_last_detection_id(), lastProximitySpaceClosureId);
statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
float x,y,z,roll,pitch,yaw;
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{
// Loop closure transform
@@ -3140,13 +3141,22 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
}
statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMapToOdom_x(), x);
statistics_.addStatistic(Statistics::kLoopMapToOdom_y(), y);
statistics_.addStatistic(Statistics::kLoopMapToOdom_z(), z);
statistics_.addStatistic(Statistics::kLoopMapToOdom_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI);
// Odom correction (actual odometry pose change)
if(!previousMapCorrection.isNull() && !odomPose.isNull())
if(!odomPose.isNull() && !previousMapCorrection.isNull())
{
Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose;
statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopOdom_correction_angle(), odomCorrection.getAngle()*180.0f/M_PI);
float x,y,z,roll,pitch,yaw;
odomCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopOdom_correction_x(), x);
statistics_.addStatistic(Statistics::kLoopOdom_correction_y(), y);
@@ -3156,6 +3166,17 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOdom_correction_yaw(), yaw*180.0f/M_PI);
}
}
if(!_lastLocalizationPose.isNull() && !_lastLocalizationPose.isIdentity())
{
_lastLocalizationPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
statistics_.addStatistic(Statistics::kLoopMapToBase_x(), x);
statistics_.addStatistic(Statistics::kLoopMapToBase_y(), y);
statistics_.addStatistic(Statistics::kLoopMapToBase_z(), z);
statistics_.addStatistic(Statistics::kLoopMapToBase_roll(), roll*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI);
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
statistics_.setLocalizationCovariance(localizationCovariance);