Skip to content

Commit

Permalink
UI: Added option to change map resolution on UI side, save/restore th…
Browse files Browse the repository at this point in the history 
…resholds in figures, fixed basic panel's slam/rgbd mode checkboxes not changing the state of corresponding parameters.
  • Loading branch information
@matlabbe
matlabbe committed Apr 13, 2024
1 parent fbbab1c commit a3267c9
Show file tree
Hide file tree
Showing 8 changed files with 208 additions and 40 deletions.
1 change: 1 addition & 0 deletions guilib/include/rtabmap/gui/PreferencesDialog.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -226,6 +226,7 @@ class RTABMAP_GUI_EXPORT PreferencesDialog : public QDialog
double getSubtractFilteringRadius() const;
double getSubtractFilteringAngle() const;

double getGridUIResolution() const;
bool getGridMapShown() const;
int getElevationMapShown() const;
int getGridMapSensor() const;
Expand Down
3 changes: 2 additions & 1 deletion guilib/include/rtabmap/gui/StatsToolBox.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,9 @@ class RTABMAP_GUI_EXPORT StatsToolBox : public QWidget
StatsToolBox(QWidget * parent);
virtual ~StatsToolBox();

void getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames);
void getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds);
void addCurve(const QString & name, bool newFigure = true, bool cacheOn = false);
void addThreshold(const QString & name, qreal value); // Add to latest figure
void setWorkingDirectory(const QString & workingDirectory);
void setNewFigureMaxItems(int value) {_newFigureMaxItems = value;}
void closeFigures();
Expand Down
8 changes: 6 additions & 2 deletions guilib/include/rtabmap/utilite/UPlot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,6 +270,7 @@ class RTABMAP_GUI_EXPORT UPlotCurveThreshold : public UPlotCurve
*/
UPlotCurveThreshold(const QString & name, qreal thesholdValue, Qt::Orientation orientation = Qt::Horizontal, QObject * parent = 0);
virtual ~UPlotCurveThreshold();
qreal getThreshold() const {return _threshold;}

public Q_SLOTS:
/**
Expand All @@ -288,6 +289,7 @@ public Q_SLOTS:

private:
Qt::Orientation _orientation;
qreal _threshold;
};

/**
Expand Down Expand Up @@ -509,8 +511,10 @@ class RTABMAP_GUI_EXPORT UPlot : public QWidget
/**
* Get all curve names.
*/
QStringList curveNames();
bool contains(const QString & curveName);
QStringList curveNames() const;
bool contains(const QString & curveName) const;
bool isThreshold(const QString & curveName) const;
double getThresholdValue(const QString & curveName) const;
void removeCurves();
QString getAllCurveDataAsText() const;
/**
Expand Down
62 changes: 59 additions & 3 deletions guilib/src/MainWindow.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,6 +264,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());

ParametersMap parameters = _preferencesDialog->getAllParameters();
// Override map resolution for UI
if(_preferencesDialog->getGridUIResolution()>0)
{
uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution())));
}
_occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters);
#ifdef RTABMAP_OCTOMAP
_octomap = new OctoMap(&_cachedLocalMaps, parameters);
Expand Down Expand Up @@ -3002,7 +3007,28 @@ void MainWindow::updateMapCloud(

jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);

_cachedLocalMaps.add(iter->first, ground, obstacles, empty, jter->sensorData().gridCellSize(), jter->sensorData().gridViewPoint());
double resolution = jter->sensorData().gridCellSize();
if(_preferencesDialog->getGridUIResolution() > jter->sensorData().gridCellSize())
{
resolution = _preferencesDialog->getGridUIResolution();
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(ground)), resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(obstacles)), resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(empty)), resolution);
if(ground.channels() == 2)
ground = util3d::laserScan2dFromPointCloud(*groundCloud).data();
else
ground = util3d::laserScanFromPointCloud(*groundCloud).data();
if(obstacles.channels() == 2)
obstacles = util3d::laserScan2dFromPointCloud(*obstaclesCloud).data();
else
obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud).data();
if(empty.channels() == 2)
empty = util3d::laserScan2dFromPointCloud(*emptyCloud).data();
else
empty = util3d::laserScanFromPointCloud(*emptyCloud).data();
}

_cachedLocalMaps.add(iter->first, ground, obstacles, empty, resolution, jter->sensorData().gridViewPoint());
}
}

Expand Down Expand Up @@ -5976,6 +6002,12 @@ void MainWindow::startDetection()
"progress will not be shown in the GUI."));
}

// Override map resolution for UI
if(_preferencesDialog->getGridUIResolution()>0)
{
uInsert(parameters, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(_preferencesDialog->getGridUIResolution())));
}

_cachedLocalMaps.clear();

delete _occupancyGrid;
Expand Down Expand Up @@ -7513,7 +7545,8 @@ void MainWindow::saveFigures()
{
QList<int> curvesPerFigure;
QStringList curveNames;
_ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames);
QStringList curveThresholds;
_ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames, curveThresholds);

QStringList curvesPerFigureStr;
for(int i=0; i<curvesPerFigure.size(); ++i)
Expand All @@ -7526,17 +7559,24 @@ void MainWindow::saveFigures()
}
_preferencesDialog->saveCustomConfig("Figures", "counts", curvesPerFigureStr.join(" "));
_preferencesDialog->saveCustomConfig("Figures", "curves", curveNames.join(" "));
_preferencesDialog->saveCustomConfig("Figures", "thresholds", curveThresholds.join(" "));
}

void MainWindow::loadFigures()
{
QString curvesPerFigure = _preferencesDialog->loadCustomConfig("Figures", "counts");
QString curveNames = _preferencesDialog->loadCustomConfig("Figures", "curves");
QString curveThresholds = _preferencesDialog->loadCustomConfig("Figures", "thresholds");

if(!curvesPerFigure.isEmpty())
{
QStringList curvesPerFigureList = curvesPerFigure.split(" ");
QStringList curvesNamesList = curveNames.split(" ");
QStringList curveThresholdsList = curveThresholds.split(" ");
if(curveThresholdsList[0].isEmpty()) {
curveThresholdsList.clear();
}
UASSERT(curveThresholdsList.isEmpty() || curveThresholdsList.size() == curvesNamesList.size());

int j=0;
for(int i=0; i<curvesPerFigureList.size(); ++i)
Expand All @@ -7553,7 +7593,23 @@ void MainWindow::loadFigures()
_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '));
for(int k=1; k<count && j<curveNames.size(); ++k)
{
_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
if(curveThresholdsList.size())
{
bool ok = false;
double thresholdValue = curveThresholdsList[j].toDouble(&ok);
if(ok)
{
_ui->statsToolBox->addThreshold(curvesNamesList[j++].replace('_', ' '), thresholdValue);
}
else
{
_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
}
}
else
{
_ui->statsToolBox->addCurve(curvesNamesList[j++].replace('_', ' '), false);
}
}
}
}
Expand Down
10 changes: 10 additions & 0 deletions guilib/src/PreferencesDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -626,6 +626,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_subtractFilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_subtractFilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));

connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_elevation_shown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
Expand Down Expand Up @@ -2005,6 +2006,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
}
else if(groupBox->objectName() == _ui->groupBox_gridMap2->objectName())
{
_ui->doubleSpinBox_map_resolution->setValue(0);
_ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
_ui->checkBox_elevation_shown->setCheckState(Qt::Unchecked);
Expand Down Expand Up @@ -2490,6 +2492,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->doubleSpinBox_subtractFilteringRadius->setValue(settings.value("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value()).toDouble());
_ui->doubleSpinBox_subtractFilteringAngle->setValue(settings.value("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value()).toDouble());

_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridUIResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
_ui->checkBox_elevation_shown->setCheckState((Qt::CheckState)settings.value("elevationMapShown", _ui->checkBox_elevation_shown->checkState()).toInt());
Expand Down Expand Up @@ -3033,6 +3036,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value());
settings.setValue("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value());

settings.setValue("gridUIResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.setValue("elevationMapShown", _ui->checkBox_elevation_shown->checkState());
Expand Down Expand Up @@ -4964,6 +4968,7 @@ void PreferencesDialog::updateBasicParameter()
{
_ui->general_checkBox_activateRGBD->blockSignals(true);
_ui->general_checkBox_activateRGBD->setChecked(_ui->general_checkBox_activateRGBD_2->isChecked());
addParameter(_ui->general_checkBox_activateRGBD, _ui->general_checkBox_activateRGBD->isChecked());
_ui->general_checkBox_activateRGBD->blockSignals(false);
}
else if(sender() == _ui->general_checkBox_SLAM_mode)
Expand All @@ -4976,6 +4981,7 @@ void PreferencesDialog::updateBasicParameter()
{
_ui->general_checkBox_SLAM_mode->blockSignals(true);
_ui->general_checkBox_SLAM_mode->setChecked(_ui->general_checkBox_SLAM_mode_2->isChecked());
addParameter(_ui->general_checkBox_SLAM_mode, _ui->general_checkBox_SLAM_mode->isChecked());
_ui->general_checkBox_SLAM_mode->blockSignals(false);
}
else
Expand Down Expand Up @@ -5899,6 +5905,10 @@ double PreferencesDialog::getSubtractFilteringAngle() const
{
return _ui->doubleSpinBox_subtractFilteringAngle->value()*M_PI/180.0;
}
double PreferencesDialog::getGridUIResolution() const
{
return _ui->doubleSpinBox_map_resolution->value();
}
bool PreferencesDialog::getGridMapShown() const
{
return _ui->checkBox_map_shown->isChecked();
Expand Down
23 changes: 22 additions & 1 deletion guilib/src/StatsToolBox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -584,10 +584,11 @@ void StatsToolBox::contextMenuEvent(QContextMenuEvent * event)
}
}

void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames)
void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds)
{
curvesPerFigure.clear();
curveNames.clear();
curveThresholds.clear();
for(QMap<QString, QWidget*>::iterator i=_figures.begin(); i!=_figures.end(); ++i)
{
QList<UPlot *> plots = i.value()->findChildren<UPlot *>();
Expand All @@ -596,6 +597,10 @@ void StatsToolBox::getFiguresSetup(QList<int> & curvesPerFigure, QStringList & c
QStringList names = plots[0]->curveNames();
curvesPerFigure.append(names.size());
curveNames.append(names);
for(int j=0; j<names.size(); ++j)
{
curveThresholds.append(plots[0]->isThreshold(names[j])?QString::number(plots[0]->getThresholdValue(names[j])):"NA");
}
}
else
{
Expand Down Expand Up @@ -628,6 +633,22 @@ void StatsToolBox::addCurve(const QString & name, bool newFigure, bool cacheOn)
ULOGGER_ERROR("Not supposed to be here...");
}
}
void StatsToolBox::addThreshold(const QString & name, qreal value)
{
QString plotName = _plotMenu->actions().last()->text();
QWidget * fig = _figures.value(plotName, (QWidget*)0);
if(fig)
{
UPlot * plot = fig->findChild<UPlot *>(plotName);
if(plot)
plot->addThreshold(name, value);
}
else
{
UERROR("There are no figures, cannot add threshold \"%s\"", name.toStdString().c_str());
}
}


void StatsToolBox::setWorkingDirectory(const QString & workingDirectory)
{
Expand Down
Loading

0 comments on commit a3267c9

Please sign in to comment.