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

odomInfoToStatistics: added more details about odometry transform parameters and ground truth

Browse Source
This commit is contained in:
matlabbe
2020-03-31 11:53:58 -04:00
parent af216ebb71
commit 7418cae8d3
1 changed files with 47 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+47 -16
View File
@@ -1151,26 +1151,57 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f)); stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f));
stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f)); stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f));
stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval)); stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval));
float dist = info.transform.getNorm();
float speed = 0.0f;
if(info.interval>0.0)
speed = dist/info.interval;
stats.insert(std::make_pair("Odometry/T/m", dist));
stats.insert(std::make_pair("Odometry/Speed/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/Speed/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/Speed/mps", speed));
stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled)); stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
float x,y,z,roll,pitch,yaw;
float dist = 0.0f, speed=0.0f;
if(!info.transform.isNull())
{
info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = info.transform.getNorm();
stats.insert(std::make_pair("Odometry/T/m", dist));
stats.insert(std::make_pair("Odometry/Tx/m", x));
stats.insert(std::make_pair("Odometry/Ty/m", y));
stats.insert(std::make_pair("Odometry/Tz/m", z));
stats.insert(std::make_pair("Odometry/Troll/deg", roll*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Tpitch/deg", pitch*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Tyaw/deg", yaw*180.0/CV_PI));
if(info.interval>0.0)
{
speed = dist/info.interval;
stats.insert(std::make_pair("Odometry/Speed/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/Speed/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/Speed/mps", speed));
}
}
if(!info.transformGroundTruth.isNull()) if(!info.transformGroundTruth.isNull())
{ {
dist = info.transformGroundTruth.getNorm(); if(!info.transform.isNull())
if(info.interval>0.0) {
speed = dist/info.interval; rtabmap::Transform diff = info.transformGroundTruth.inverse()*info.transform;
stats.insert(std::make_pair("Odometry/TG/m", dist)); stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
stats.insert(std::make_pair("Odometry/SpeedG/kph", speed*3.6)); stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/SpeedG/mph", speed*2.237)); }
stats.insert(std::make_pair("Odometry/SpeedG/mps", speed));
}
info.transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = info.transformGroundTruth.getNorm();
stats.insert(std::make_pair("Odometry/TG/m", dist));
stats.insert(std::make_pair("Odometry/TGx/m", x));
stats.insert(std::make_pair("Odometry/TGy/m", y));
stats.insert(std::make_pair("Odometry/TGz/m", z));
stats.insert(std::make_pair("Odometry/TGroll/deg", roll*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TGpitch/deg", pitch*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TGyaw/deg", yaw*180.0/CV_PI));
if(info.interval>0.0)
{
speed = dist/info.interval;
stats.insert(std::make_pair("Odometry/SpeedG/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/SpeedG/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/SpeedG/mps", speed));
}
}
return stats; return stats;
} }