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

rtabmap: Saving more odometry statistics to database

Browse Source
This commit is contained in:
matlabbe
2019-11-26 18:33:05 -05:00
parent cf2f16dd38
commit 954836c58b
3 changed files with 45 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/MsgConversion.h
+1
View File
@@ -152,6 +152,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg); rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg); rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg); void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
src/CoreWrapper.cpp
+2 -34
View File
@@ -1713,23 +1713,7 @@ void CoreWrapper::process(
if(iter->second.timeEstimation != 0.0f) if(iter->second.timeEstimation != 0.0f)
{ {
OdometryInfo info = odomInfoFromROS(iter->second); OdometryInfo info = odomInfoFromROS(iter->second);
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", info.localBundleTime*1000.0f)); externalStats = rtabmap_ros::odomInfoToStatistics(info);
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", info.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", info.reg.totalTime*1000.0f));
float speed = 0.0f;
if(info.interval>0.0)
speed = info.transform.x()/info.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", info.features));
externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", info.distanceTravelled));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded));
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
if(info.interval>0.0) if(info.interval>0.0)
{ {
@@ -1876,23 +1860,7 @@ void CoreWrapper::process(
std::vector<float> odomVelocity; std::vector<float> odomVelocity;
if(odomInfo.timeEstimation != 0.0f) if(odomInfo.timeEstimation != 0.0f)
{ {
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f)); externalStats = rtabmap_ros::odomInfoToStatistics(odomInfo);
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
float speed = 0.0f;
if(odomInfo.interval>0.0)
speed = odomInfo.transform.x()/odomInfo.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage));
if(odomInfo.interval>0.0) if(odomInfo.interval>0.0)
{ {
src/MsgConversion.cpp
+42
View File
@@ -1110,6 +1110,48 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose); transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
} }
std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info)
{
std::map<std::string, float> stats;
stats.insert(std::make_pair("Odometry/TimeRegistration/ms", info.reg.totalTime*1000.0f));
stats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
// Based on rtabmap/MainWindow.cpp
stats.insert(std::make_pair("Odometry/Features/", info.features));
stats.insert(std::make_pair("Odometry/Matches/", info.reg.matches));
stats.insert(std::make_pair("Odometry/MatchesRatio/", info.features<=0?0.0f:float(info.reg.inliers)/float(info.features)));
stats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", info.reg.inliersMeanDistance));
stats.insert(std::make_pair("Odometry/InliersDistribution/", info.reg.inliersDistribution));
stats.insert(std::make_pair("Odometry/InliersRatio/", info.reg.inliers));
stats.insert(std::make_pair("Odometry/ICPInliersRatio/", info.reg.icpInliersRatio));
stats.insert(std::make_pair("Odometry/ICPRotation/rad", info.reg.icpRotation));
stats.insert(std::make_pair("Odometry/ICPTranslation/m", info.reg.icpTranslation));
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
stats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
stats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
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/Interval/ms", (float)info.interval));
float speed = 0.0f;
if(info.interval>0.0)
speed = info.transform.x()/info.interval*3.6;
stats.insert(std::make_pair("Odometry/Speed/kph", speed));
stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
return stats;
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg) rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{ {
rtabmap::OdometryInfo info; rtabmap::OdometryInfo info;