From f161609a2486b6cb72826894bec37a2db77db5c0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 16 Jul 2016 10:57:59 -0400 Subject: [PATCH] Added octomap ground is an obstacle option --- .../include/rtabmap/gui/PreferencesDialog.h | 1 + guilib/src/MainWindow.cpp | 5 ++ guilib/src/PreferencesDialog.cpp | 11 +++- guilib/src/ui/preferencesDialog.ui | 51 ++++++++++++++----- 4 files changed, 53 insertions(+), 15 deletions(-) diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 18b17a35..25263063 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -154,6 +154,7 @@ public: bool isCloudsShown(int index) const; // 0=map, 1=odom bool isOctomapShown() const; int getOctomapTreeDepth() const; + bool isOctomapGroundAnObstacle() const; int getCloudDecimation(int index) const; // 0=map, 1=odom double getCloudMaxDepth(int index) const; // 0=map, 1=odom double getCloudMinDepth(int index) const; // 0=map, 1=odom diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 8491c605..7a363f8a 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2646,6 +2646,11 @@ void MainWindow::createAndAddProjectionMap( groundCloud = util3d::transformPointCloud(groundCloud, tinv); obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); } + if(_preferencesDialog->isOctomapGroundAnObstacle()) + { + *obstaclesCloud += *groundCloud; + groundCloud->clear(); + } _octomap->addToCache(nodeId, groundCloud, obstaclesCloud); } #endif diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 371774ad..e0e3159d 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -159,7 +159,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->label_octomap->setEnabled(false); _ui->spinBox_octomap_treeDepth->setEnabled(false); _ui->label_octomap_treeDepth->setEnabled(false); - + _ui->checkBox_octomap_groundObstacle->setEnabled(false); + _ui->label_octomap_groundObstacle->setEnabled(false); #endif #ifndef RTABMAP_NONFREE @@ -390,6 +391,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_octomap, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_octomap_groundObstacle, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->groupBox_organized, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -1228,6 +1230,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_octomap->setChecked(false); _ui->spinBox_octomap_treeDepth->setValue(16); + _ui->checkBox_octomap_groundObstacle->setChecked(true); _ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0); #if PCL_VERSION_COMPARE(>=, 1, 7, 2) @@ -1589,6 +1592,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->checkBox_octomap->setChecked(settings.value("octomap", _ui->checkBox_octomap->isChecked()).toBool()); _ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt()); + _ui->checkBox_octomap_groundObstacle->setChecked(settings.value("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked()).toBool()); _ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool()); _ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble()); @@ -1999,6 +2003,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("octomap", _ui->checkBox_octomap->isChecked()); settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value()); + settings.setValue("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked()); settings.setValue("meshing", _ui->groupBox_organized->isChecked()); settings.setValue("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()); @@ -3663,6 +3668,10 @@ int PreferencesDialog::getOctomapTreeDepth() const { return _ui->spinBox_octomap_treeDepth->value(); } +bool PreferencesDialog::isOctomapGroundAnObstacle() const +{ + return _ui->checkBox_octomap_groundObstacle->isChecked(); +} double PreferencesDialog::getMapVoxel() const { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index e885fce4..fef643ea 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -464 + -568 686 2023 @@ -785,6 +785,19 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + m + + + 2 + + + 9999.000000000000000 + + + @@ -928,19 +941,6 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - m - - - 2 - - - 9999.000000000000000 - - - @@ -990,6 +990,29 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Octomap: ground is an obstacle. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + true + + +