From 451211cfb1f3529602d0f335278c6bb16c125b45 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 28 Mar 2020 17:07:22 -0400 Subject: [PATCH] OdomInfo: added icpCorrespondences. Stats: using norm instead of x for distance/speed. Added odometry speed statistic in mph --- .../include/rtabmap/core/RegistrationInfo.h | 1 + corelib/src/RegistrationIcp.cpp | 1 + guilib/src/MainWindow.cpp | 26 ++++++++++++++----- 3 files changed, 21 insertions(+), 7 deletions(-) diff --git a/corelib/include/rtabmap/core/RegistrationInfo.h b/corelib/include/rtabmap/core/RegistrationInfo.h index 3782880c..c8baedb6 100644 --- a/corelib/include/rtabmap/core/RegistrationInfo.h +++ b/corelib/include/rtabmap/core/RegistrationInfo.h @@ -83,6 +83,7 @@ public: float icpTranslation; float icpRotation; float icpStructuralComplexity; + int icpCorrespondences; }; } diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index 48b8c0c6..7b3d7b21 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -1222,6 +1222,7 @@ Transform RegistrationIcp::computeTransformationImpl( info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance; } info.icpInliersRatio = correspondencesRatio; + info.icpCorrespondences = correspondences; if(correspondencesRatio <= _correspondenceRatio) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 8fb32ab8..2d60f82d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -612,6 +612,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", false); _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", false); _ui->statsToolBox->updateStat("Odometry/Distance/m", false); + _ui->statsToolBox->updateStat("Odometry/T/m", false); _ui->statsToolBox->updateStat("Odometry/Tx/m", false); _ui->statsToolBox->updateStat("Odometry/Ty/m", false); _ui->statsToolBox->updateStat("Odometry/Tz/m", false); @@ -1500,6 +1501,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPStructuralComplexity/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpStructuralComplexity, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.icpCorrespondences, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at(0,0)), _preferencesDialog->isCacheSavedInFigures()); @@ -1518,10 +1520,12 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures()); - float x=0.0f,y,z, roll,pitch,yaw; + float dist=0.0f, x,y,z, roll,pitch,yaw; if(!odom.info().transform.isNull()) { odom.info().transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + dist = odom.info().transform.getNorm(); + _ui->statsToolBox->updateStat("Odometry/T/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Tx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Ty/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Tz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures()); @@ -1533,6 +1537,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI if(!odom.info().transformFiltered.isNull()) { odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + dist = odom.info().transformFiltered.getNorm(); + _ui->statsToolBox->updateStat("Odometry/TF/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TFx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TFy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TFz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures()); @@ -1543,20 +1549,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI if(odom.info().interval > 0) { _ui->statsToolBox->updateStat("Odometry/Interval/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().interval*1000.f, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/Speed/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/Speed/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/Speed/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures()); } if(!odom.info().guessVelocity.isNull()) { odom.info().guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); - _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x*3.6f, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures()); + dist = odom.info().guessVelocity.getNorm(); + _ui->statsToolBox->updateStat("Odometry/SpeedGuess/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*3.6f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist*2.237f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/SpeedGuess/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures()); } if(!odom.info().transformGroundTruth.isNull()) { odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + dist = odom.info().transformGroundTruth.getNorm(); + _ui->statsToolBox->updateStat("Odometry/TG/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TGx/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TGy/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), y, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TGz/m", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), z, _preferencesDialog->isCacheSavedInFigures()); @@ -1565,8 +1576,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI _ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures()); if(odom.info().interval > 0) { - _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures()); - _ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/SpeedG/mph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval*2.237f, _preferencesDialog->isCacheSavedInFigures()); + _ui->statsToolBox->updateStat("Odometry/SpeedG/mps", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), dist/odom.info().interval, _preferencesDialog->isCacheSavedInFigures()); } }