diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 155819b1c8..a593acc37d 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/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; diff --git a/guilib/include/rtabmap/gui/StatsToolBox.h b/guilib/include/rtabmap/gui/StatsToolBox.h index 86ab8c7be9..b08b7cdfd5 100644 --- a/guilib/include/rtabmap/gui/StatsToolBox.h +++ b/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 & curvesPerFigure, QStringList & curveNames); + void getFiguresSetup(QList & 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(); diff --git a/guilib/include/rtabmap/utilite/UPlot.h b/guilib/include/rtabmap/utilite/UPlot.h index 3d78729211..c3d4cd1250 100644 --- a/guilib/include/rtabmap/utilite/UPlot.h +++ b/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; /** diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 0d9a3d6ee9..b5cde034d0 100644 --- a/guilib/src/MainWindow.cpp +++ b/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::Ptr groundCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(ground)), resolution); + pcl::PointCloud::Ptr obstaclesCloud = util3d::voxelize(util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(obstacles)), resolution); + pcl::PointCloud::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 curvesPerFigure; QStringList curveNames; - _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames); + QStringList curveThresholds; + _ui->statsToolBox->getFiguresSetup(curvesPerFigure, curveNames, curveThresholds); QStringList curvesPerFigureStr; for(int i=0; isaveCustomConfig("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; istatsToolBox->addCurve(curvesNamesList[j++].replace('_', ' ')); for(int k=1; kstatsToolBox->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); + } } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 8612784adc..07f4d5215a 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/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(); diff --git a/guilib/src/StatsToolBox.cpp b/guilib/src/StatsToolBox.cpp index 6a7e586316..54933e944a 100644 --- a/guilib/src/StatsToolBox.cpp +++ b/guilib/src/StatsToolBox.cpp @@ -584,10 +584,11 @@ void StatsToolBox::contextMenuEvent(QContextMenuEvent * event) } } -void StatsToolBox::getFiguresSetup(QList & curvesPerFigure, QStringList & curveNames) +void StatsToolBox::getFiguresSetup(QList & curvesPerFigure, QStringList & curveNames, QStringList & curveThresholds) { curvesPerFigure.clear(); curveNames.clear(); + curveThresholds.clear(); for(QMap::iterator i=_figures.begin(); i!=_figures.end(); ++i) { QList plots = i.value()->findChildren(); @@ -596,6 +597,10 @@ void StatsToolBox::getFiguresSetup(QList & curvesPerFigure, QStringList & c QStringList names = plots[0]->curveNames(); curvesPerFigure.append(names.size()); curveNames.append(names); + for(int j=0; jisThreshold(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(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) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 16a484557b..8cbd62324c 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -95,7 +95,7 @@ QFrame::Raised - 11 + 3 @@ -2414,7 +2414,33 @@ Show a yellow background when the number of odometry inliers goes under this thr - + + + + Show in 3D map view. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Resolution. Set 0 to use same value from Local Occupancy Grid panel. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + @@ -2433,60 +2459,78 @@ Show a yellow background when the number of odometry inliers goes under this thr - - + + - Show in 3D map view. + - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + false - - - - Opacity. - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + Partially checked: Height color map. Checked: RGB color map. - - - - false + + true + - + Show Elevation Map in 3D map view. + + true + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse - - - - Partially checked: Height color map. Checked: RGB color map. - + + - - - - false + Opacity. - + true + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 10.000000000000000 + + + 0.010000000000000 + + + 0.000000000000000 + diff --git a/guilib/src/utilite/UPlot.cpp b/guilib/src/utilite/UPlot.cpp index 26f275d35c..3377dbf3e3 100644 --- a/guilib/src/utilite/UPlot.cpp +++ b/guilib/src/utilite/UPlot.cpp @@ -1055,6 +1055,7 @@ UPlotCurveThreshold::UPlotCurveThreshold(const QString & name, qreal thesholdVal UPlotCurve(name, parent), _orientation(orientation) { + _threshold = thesholdValue; if(_orientation == Qt::Horizontal) { this->addValue(0, thesholdValue); @@ -1080,6 +1081,7 @@ void UPlotCurveThreshold::setThreshold(qreal threshold) if(_items.size() == 3) { UPlotItem * item = 0; + _threshold = threshold; if(_orientation == Qt::Horizontal) { item = (UPlotItem*)_items.at(0); @@ -2193,10 +2195,10 @@ bool UPlot::addCurve(UPlotCurve * curve, bool ownershipTransferred) return false; } -QStringList UPlot::curveNames() +QStringList UPlot::curveNames() const { QStringList names; - for(QList::iterator iter = _curves.begin(); iter!=_curves.end(); ++iter) + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) { if(*iter) { @@ -2206,9 +2208,38 @@ QStringList UPlot::curveNames() return names; } -bool UPlot::contains(const QString & curveName) +bool UPlot::isThreshold(const QString & curveName) const { - for(QList::iterator iter = _curves.begin(); iter!=_curves.end(); ++iter) + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) + { + if(*iter && (*iter)->name().compare(curveName) == 0) + { + return qobject_cast(*iter) != 0; + } + } + return false; +} + +double UPlot::getThresholdValue(const QString & curveName) const +{ + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) + { + if(*iter && (*iter)->name().compare(curveName) == 0) + { + UPlotCurveThreshold* curve = qobject_cast(*iter); + if(curve) + { + return curve->getThreshold(); + } + } + } + UERROR("Curve \"%s\" not found as theshold!", curveName.toStdString().c_str()); + return 0; +} + +bool UPlot::contains(const QString & curveName) const +{ + for(QList::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd(); ++iter) { if(*iter && (*iter)->name().compare(curveName) == 0) {