diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 91bcb14e..0cbb43c7 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -271,6 +271,7 @@ public: bool isSLAMMode() const; bool isRGBDMode() const; int getKpMaxFeatures() const; + bool isPriorIgnored() const; //specific bool isStatisticsPublished() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 29005a62..7a34a359 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1905,6 +1905,22 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) uInsert(labels, std::pair(*iter)); // overwrite labels because they could have been modified } + if(_preferencesDialog->isPriorIgnored() && + _ui->graphicsView_graphView->getWorldMapRotation()==0.0f && + stat.getLastSignatureData().sensorData().gps().stamp()!=0.0 && + stat.poses().find(stat.getLastSignatureData().id())!=stat.poses().end()) + { + float bearing = (float)((-(stat.getLastSignatureData().sensorData().gps().bearing()-90))*M_PI/180.0); + float gpsRotationOffset = stat.poses().at(stat.getLastSignatureData().id()).theta()-bearing; + _ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset); + } + else if(!_preferencesDialog->isPriorIgnored() && + _ui->graphicsView_graphView->getWorldMapRotation() != 0.0f) + { + _ui->graphicsView_graphView->setWorldMapRotation(0.0f); + } + + std::map poses = stat.poses(); UDEBUG("time= %d ms", time.restart()); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index f79692f2..d683e097 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -5422,6 +5422,10 @@ int PreferencesDialog::getKpMaxFeatures() const { return _ui->surf_spinBox_wordsPerImageTarget->value(); } +bool PreferencesDialog::isPriorIgnored() const +{ + return _ui->graphOptimization_priorsIgnored->isChecked(); +} /*** SETTERS ***/ void PreferencesDialog::setInputRate(double value)