UI: Added option to change map resolution on UI side, save/restore th…

…resholds in figures, fixed basic panel's slam/rgbd mode checkboxes not changing the state of corresponding parameters.

guilib/include/rtabmap/gui/PreferencesDialog.h

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

guilib/include/rtabmap/gui/StatsToolBox.h

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

guilib/include/rtabmap/utilite/UPlot.h

@@ -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:
  /**
@@ -288,6 +289,7 @@ public Q_SLOTS:
 
 private:
  Qt::Orientation _orientation;
+ qreal _threshold;
 };
 
 /**
@@ -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;
  /**

guilib/src/MainWindow.cpp

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

guilib/src/PreferencesDialog.cpp

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

guilib/src/StatsToolBox.cpp

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