diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index e63b18b5..dfb5f929 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -162,6 +162,7 @@ public: bool isPosteriorGraphView() const; bool isWordsCountGraphView() const; bool isLocalizationsCountGraphView() const; + bool isRelocalizationColorOdomCacheGraphView() const; int getOdomRegistrationApproach() const; double getOdomF2MGravitySigma() const; bool isOdomDisabled() const; diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index f9d048d7..745300bf 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -707,7 +707,7 @@ void GraphViewer::updateGraph(const std::map & poses, if(_nodeItems.size()) { - (--_nodeItems.end()).value()->setColor(Qt::green); + (--_nodeItems.end()).value()->setColor(_nodeOdomCacheColor); } this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents @@ -1064,10 +1064,6 @@ void GraphViewer::updatePosterior(const std::map & posterior, float iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue iter.value()->setZValue(iter.value()->zValue()+zValueOffset); } - else if(iter.key() > 0) - { - iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue - } } } } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 1a0b0988..c76a0db5 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1576,7 +1576,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI #if PCL_VERSION_COMPARE(>=, 1, 7, 2) if(_preferencesDialog->isFramesShown()) { - _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), false, false); + _cloudViewer->addOrUpdateLine("odom_to_base_link", _odometryCorrection, _odometryCorrection*odom.pose(), qRgb(255, 128, 0), true, false); } else { @@ -2391,21 +2391,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UDEBUG("time= %d ms (update gt-gps stuff)", time.restart()); -#if PCL_VERSION_COMPARE(>=, 1, 7, 2) - if(_preferencesDialog->isFramesShown()) - { - _cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false); - _cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false); - _cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), false, false); - } - else - { - _cloudViewer->removeLine("map_to_odom"); - _cloudViewer->removeCoordinate("odom_frame"); - _cloudViewer->removeCoordinate("map_frame"); - } -#endif - UDEBUG("%d %d %d", poses.size(), poses.size()?poses.rbegin()->first:0, stat.refImageId()); if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId()) { @@ -2489,6 +2474,28 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures()); } } + +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) + if(_preferencesDialog->isFramesShown()) + { + _cloudViewer->addOrUpdateCoordinate("map_frame", Transform::getIdentity(), 0.5, false); + _cloudViewer->addOrUpdateCoordinate("odom_frame", _odometryCorrection, 0.35, false); + _cloudViewer->addOrUpdateLine("map_to_odom", Transform::getIdentity(), _odometryCorrection, qRgb(255, 128, 0), true, false); + if(_preferencesDialog->isLabelsShown()) + { + _cloudViewer->addOrUpdateText("map_frame_label", "map", Transform::getIdentity(), 0.1, Qt::white); + _cloudViewer->addOrUpdateText("odom_frame_label", "odom", _odometryCorrection, 0.1, Qt::white); + } + } + else + { + _cloudViewer->removeLine("map_to_odom"); + _cloudViewer->removeCoordinate("odom_frame"); + _cloudViewer->removeCoordinate("map_frame"); + _cloudViewer->removeText("map_frame_label"); + _cloudViewer->removeText("odom_frame_label"); + } +#endif } if( _ui->graphicsView_graphView->isVisible()) @@ -2512,6 +2519,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _ui->graphicsView_graphView->updatePosterior(_cachedLocalizationsCount, 1.0f); } } + if(_preferencesDialog->isRelocalizationColorOdomCacheGraphView() && !stat.odomCachePoses().empty()) + { + std::map colors; + colors.insert(std::make_pair(stat.odomCachePoses().rbegin()->first, 240)); + for(std::multimap::const_iterator iter=stat.odomCacheConstraints().begin(); iter!=stat.odomCacheConstraints().end(); ++iter) + { + if(iter->second.type() != Link::kNeighbor) + { + uInsert(colors, std::pair(iter->second.from()>iter->second.to()?iter->second.from():iter->second.to(), 120)); //green + } + } + for(std::map::const_iterator iter=stat.odomCachePoses().begin(); iter!=stat.odomCachePoses().end(); ++iter) + { + if(stat.poses().find(iter->first) == stat.poses().end()) + { + colors.insert(std::make_pair(iter->first, 240)); //red + } + } + _ui->graphicsView_graphView->updatePosterior(colors, 240); + } // update local path on the graph view _ui->graphicsView_graphView->updateLocalPath(stat.localPath()); if(stat.localPath().size() == 0) @@ -5224,6 +5251,8 @@ void MainWindow::newDatabase() _cloudViewer->removeLine("odom_to_base_link"); _cloudViewer->removeCoordinate("odom_frame"); _cloudViewer->removeCoordinate("map_frame"); + _cloudViewer->removeText("map_frame_label"); + _cloudViewer->removeText("odom_frame_label"); ULOGGER_DEBUG(""); this->clearTheCache(); std::string databasePath = (_preferencesDialog->getWorkingDirectory()+QDir::separator()+QString("rtabmap.tmp.db")).toStdString(); @@ -5373,8 +5402,9 @@ void MainWindow::openDatabase(const QString & path, const ParametersMap & overri if(different) { differentParameters.insert(*iter); - QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"") + QString msg = tr("Parameter \"%1\": %2=\"%3\" Preferences=\"%4\"") .arg(iter->first.c_str()) + .arg(overridedParameters.find(iter->first) != overridedParameters.end()?"Arguments":"Database") .arg(iter->second.c_str()) .arg(jter->second.c_str()); _ui->widget_console->appendMsg(msg); @@ -5387,9 +5417,11 @@ void MainWindow::openDatabase(const QString & path, const ParametersMap & overri { int r = QMessageBox::question(this, tr("Update parameters..."), - tr("The database is using %1 different parameter(s) than " + tr("The %1 using %2 different parameter(s) than " "those currently set in Preferences. Do you want " - "to use database's parameters?").arg(differentParameters.size()), + "to use those parameters?") + .arg(overridedParameters.empty()?tr("database is"):tr("database and input arguments are")) + .arg(differentParameters.size()), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes); if(r == QMessageBox::Yes) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 67acf51e..270ccd4c 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -463,6 +463,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->radioButton_posteriorGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel())); connect(_ui->radioButton_wordsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel())); connect(_ui->radioButton_localizationsGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel())); + connect(_ui->radioButton_localizationsGraphViewOdomCache, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel())); connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel())); connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel())); connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel())); @@ -1803,6 +1804,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->radioButton_posteriorGraphView->setChecked(true); _ui->radioButton_wordsGraphView->setChecked(false); _ui->radioButton_localizationsGraphView->setChecked(false); + _ui->radioButton_localizationsGraphViewOdomCache->setChecked(false); _ui->radioButton_nochangeGraphView->setChecked(false); _ui->checkbox_odomDisabled->setChecked(false); _ui->checkbox_groundTruthAlign->setChecked(true); @@ -2287,6 +2289,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->radioButton_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->radioButton_posteriorGraphView->isChecked()).toBool()); _ui->radioButton_wordsGraphView->setChecked(settings.value("wordsGraphView", _ui->radioButton_wordsGraphView->isChecked()).toBool()); _ui->radioButton_localizationsGraphView->setChecked(settings.value("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked()).toBool()); + _ui->radioButton_localizationsGraphViewOdomCache->setChecked(settings.value("localizationsGraphViewOdomCache", _ui->radioButton_localizationsGraphViewOdomCache->isChecked()).toBool()); _ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool()); _ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool()); _ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt()); @@ -2814,6 +2817,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("posteriorGraphView", _ui->radioButton_posteriorGraphView->isChecked()); settings.setValue("wordsGraphView", _ui->radioButton_wordsGraphView->isChecked()); settings.setValue("localizationsGraphView", _ui->radioButton_localizationsGraphView->isChecked()); + settings.setValue("localizationsGraphViewOdomCache", _ui->radioButton_localizationsGraphViewOdomCache->isChecked()); settings.setValue("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()); settings.setValue("odomDisabled", _ui->checkbox_odomDisabled->isChecked()); settings.setValue("odomRegistration", _ui->odom_registration->currentIndex()); @@ -5331,6 +5335,10 @@ bool PreferencesDialog::isLocalizationsCountGraphView() const { return _ui->radioButton_localizationsGraphView->isChecked(); } +bool PreferencesDialog::isRelocalizationColorOdomCacheGraphView() const +{ + return _ui->radioButton_localizationsGraphViewOdomCache->isChecked(); +} bool PreferencesDialog::isOdomDisabled() const { return _ui->checkbox_odomDisabled->isChecked(); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 56fc7c42..e9b98616 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -853 + -166 756 3657 @@ -95,7 +95,7 @@ QFrame::Raised - 21 + 0 @@ -404,105 +404,161 @@ Graph View - - - - - Show loop closure hypotheses on the graph view. Nodes are colorized from blue to red depending on the highest hypothesis (which is red). Posterior hypotheses should be published. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + Map Graph + + + + + + + + true + + + + + + + Show loop closure hypotheses on the graph view. Nodes are colorized from blue to red depending on the highest hypothesis (which is red). Posterior hypotheses should be published. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Show 3D visual words count on the graph view. Nodes are colorized from blue to red depending on the maximum visual features extracted (which is red). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + + + + + + Show nodes on which loop closures or localizations happened in red. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + + + + + No change of graph color. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - - - - - - - false - - - - - - - - - - true - - - - - - - Show 3D visual words count on the graph view. Nodes are colorized from blue to red depending on the maximum visual features extracted (which is red). - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - - - - - No change of graph color. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - Show nodes on which loop closures or localizations happened in red. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - - - - - - - false + + + + Localization Graph + + + + + + + + false + + + + + + + Show nodes in green when there is re-localization, red otherwise. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + + + + + + No change of graph color. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + - radioButton_posteriorGraphView - radioButton_wordsGraphView - label_478 - label_222 - label_479 - radioButton_nochangeGraphView - label_481 - radioButton_localizationsGraphView