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

Refactoring of the cameraStereoImages and cameraRGBDImages classes (now inheriting from CameraImages) for easy setting of laser scan path, timestamps path and ground truth path.

Browse Source 
Added graph::importPoses().
Can now have a ground truth published with SensorData (filled optionally by CameraImages classes).
Increased database closing time performance when the database is not saved.
Added ParametersToolBox widget in DatabaseViewer for core parameters (refactoring done to make easy access to all rtabmap parameters in DatabaseViewer).
Added Parameters::getType(key).
Added Transform::interpolate() to interpolate between two transforms (SLERP)
Updated pf_filter.m and added test_pf_filter.m MATLAB scripts (making easier to compare with a ground truth)
UPlot: can now save all curve data of a figure in one action (see right-click on legend area->"Copy all curve data to clipboard")
This commit is contained in:
matlabbe
2015-12-17 14:13:19 -05:00
parent f5c062448d
commit 3292fb1146
49 changed files with 2135 additions and 2010 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/MainWindow.cpp
+53 -23
View File
@@ -145,7 +145,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_waypointsIndex(0),
_odometryCorrection(Transform::getIdentity()),
_processingOdometry(false),
_lastOdomInfoUpdateTime(0),
_oneSecondTimer(0),
_elapsedTime(0),
_posteriorCurve(0),
@@ -509,6 +508,7 @@ MainWindow::~MainWindow()
this->stopDetection();
delete _ui;
delete _elapsedTime;
UDEBUG("");
}
void MainWindow::setupMainLayout(bool vertical)
@@ -698,22 +698,19 @@ void MainWindow::handleEvent(UEvent* anEvent)
}
else if(anEvent->getClassName().compare("OdometryEvent") == 0)
{
// limit 10 Hz max
if(UTimer::now() - _lastOdomInfoUpdateTime > 0.1)
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if(!_processingOdometry && !_processingStatistics)
{
_lastOdomInfoUpdateTime = UTimer::now();
OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
if(!_processingOdometry && !_processingStatistics)
{
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(*odomEvent);
}
else
{
// we receive too many odometry events! just send without data
OdometryEvent tmp(SensorData(cv::Mat(), odomEvent->data().id()), odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
emit odometryReceived(tmp);
}
_processingOdometry = true; // if we receive too many odometry events!
emit odometryReceived(*odomEvent);
}
else
{
// we receive too many odometry events! just send without data
SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
data.setGroundTruth(odomEvent->data().groundTruth());
OdometryEvent tmp(data, odomEvent->pose(), odomEvent->covariance(), odomEvent->info());
emit odometryReceived(tmp);
}
}
else if(anEvent->getClassName().compare("ULogEvent") == 0)
@@ -1034,12 +1031,45 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
if(!odom.info().transformFiltered.isNull())
{
odom.info().transformFiltered.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Fx/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/Fy/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/Fz/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/Froll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Fpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Fyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TFx/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/TFy/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/TFz/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/TFroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TFpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TFyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
if(!odom.info().transformGroundTruth.isNull())
{
odom.info().transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/TGx/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/TGy/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/TGz/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/TGroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
//cumulative pose
if(!odom.pose().isNull())
{
odom.pose().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/Px/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/Py/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/Pz/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/Proll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Ppitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/Pyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
if(!odom.data().groundTruth().isNull())
{
odom.data().groundTruth().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
_ui->statsToolBox->updateStat("Odometry/PGx/m", (float)odom.data().id(), x);
_ui->statsToolBox->updateStat("Odometry/PGy/m", (float)odom.data().id(), y);
_ui->statsToolBox->updateStat("Odometry/PGz/m", (float)odom.data().id(), z);
_ui->statsToolBox->updateStat("Odometry/PGroll/deg", (float)odom.data().id(), roll*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/PGpitch/deg", (float)odom.data().id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/PGyaw/deg", (float)odom.data().id(), yaw*180.0/CV_PI);
}
if(odom.info().interval > 0)
@@ -2826,7 +2856,7 @@ bool MainWindow::closeDatabase()
}
}
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose));
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdClose, !_newDatabasePathOutput.isEmpty()));
return true;
}