i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Gui: Added re-localization colors on odom cache graph. Added frame labels (map, odom).

Browse Source
This commit is contained in:
matlabbe
2022-10-04 16:27:33 -07:00
parent 7e3e5ec50b
commit 1e9b090624
5 changed files with 211 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/src/GraphViewer.cpp
View File

@@ -707,7 +707,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & 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<int, float> & 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
}
}
}
}

70
guilib/src/MainWindow.cpp
View File

@@ -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<int, float> colors;
colors.insert(std::make_pair(stat.odomCachePoses().rbegin()->first, 240));
for(std::multimap<int, Link>::const_iterator iter=stat.odomCacheConstraints().begin(); iter!=stat.odomCacheConstraints().end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor)
{
uInsert(colors, std::pair<int,float>(iter->second.from()>iter->second.to()?iter->second.from():iter->second.to(), 120)); //green
}
}
for(std::map<int, Transform>::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)

8
guilib/src/PreferencesDialog.cpp
View File

@@ -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();

244
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-853</y>
<y>-166</y>
<width>756</width>
<height>3657</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>21</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -404,105 +404,161 @@
<property name="title">
<string>Graph View</string>
</property>
<layout class="QGridLayout" name="gridLayout_104" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_222">
<property name="text">
<string>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. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<layout class="QVBoxLayout" name="verticalLayout_167">
<item>
<widget class="QGroupBox" name="groupBox_34">
<property name="title">
<string>Map Graph</string>
</property>
<layout class="QGridLayout" name="gridLayout_104" columnstretch="0,1">
<item row="0" column="0">
<widget class="QRadioButton" name="radioButton_posteriorGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_222">
<property name="text">
<string>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. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QRadioButton" name="radioButton_wordsGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_478">
<property name="text">
<string>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).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QRadioButton" name="radioButton_localizationsGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_481">
<property name="text">
<string>Show nodes on which loop closures or localizations happened in red.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QRadioButton" name="radioButton_nochangeGraphView">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_479">
<property name="text">
<string>No change of graph color.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item row="1" column="0">
<widget class="QRadioButton" name="radioButton_wordsGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QRadioButton" name="radioButton_posteriorGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_478">
<property name="text">
<string>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).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QRadioButton" name="radioButton_nochangeGraphView">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_479">
<property name="text">
<string>No change of graph color.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_481">
<property name="text">
<string>Show nodes on which loop closures or localizations happened in red.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QRadioButton" name="radioButton_localizationsGraphView">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
<item>
<widget class="QGroupBox" name="groupBox_35">
<property name="title">
<string>Localization Graph</string>
</property>
<layout class="QGridLayout" name="gridLayout_123" columnstretch="0,1">
<item row="0" column="0">
<widget class="QRadioButton" name="radioButton_localizationsGraphViewOdomCache">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_649">
<property name="text">
<string>Show nodes in green when there is re-localization, red otherwise.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QRadioButton" name="radioButton_nochangeGraphViewOdomCache">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_648">
<property name="text">
<string>No change of graph color.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
<zorder>radioButton_posteriorGraphView</zorder>
<zorder>radioButton_wordsGraphView</zorder>
<zorder>label_478</zorder>
<zorder>label_222</zorder>
<zorder>label_479</zorder>
<zorder>radioButton_nochangeGraphView</zorder>
<zorder>label_481</zorder>
<zorder>radioButton_localizationsGraphView</zorder>
</widget>
</item>
<item>