diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 110218be..9594457f 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -232,7 +232,9 @@ private: bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const; + float regenerateMaxDepth, + float filteringRadius, + float filteringMinNeighbors) const; bool getExportedScans(std::map::Ptr > & scans); bool getExportedClouds( diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index ee4ebb30..eceb181e 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -51,6 +51,10 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->groupBox_filtering, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_filteringRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->spinBox_filteringMinNeighbors, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->groupBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -102,6 +106,10 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("regenerate_voxel", this->getGenerateVoxel()); settings.setValue("regenerate_max_depth", this->getGenerateMaxDepth()); + settings.setValue("filtering", this->getFiltering()); + settings.setValue("filtering_radius", this->getFilteringRadius()); + settings.setValue("filtering_min_neighbors", this->getFilteringMinNeighbors()); + settings.setValue("assemble", this->getAssemble()); settings.setValue("assemble_voxel", this->getAssembleVoxel()); @@ -143,6 +151,10 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->doubleSpinBox_voxelSize->setValue(settings.value("regenerate_voxel", this->getGenerateVoxel()).toDouble()); _ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", this->getGenerateMaxDepth()).toDouble()); + _ui->groupBox_filtering->setChecked(settings.value("filtering", this->getFiltering()).toBool()); + _ui->doubleSpinBox_filteringRadius->setValue(settings.value("filtering_radius", this->getFilteringRadius()).toDouble()); + _ui->spinBox_filteringMinNeighbors->setValue(settings.value("filtering_min_neighbors", this->getFilteringMinNeighbors()).toInt()); + _ui->groupBox_assemble->setChecked(settings.value("assemble", this->getAssemble()).toBool()); _ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", this->getAssembleVoxel()).toDouble()); @@ -182,6 +194,10 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_voxelSize->setValue(0.01); _ui->doubleSpinBox_maxDepth->setValue(4); + _ui->groupBox_filtering->setChecked(false); + _ui->doubleSpinBox_filteringRadius->setValue(0.02); + _ui->spinBox_filteringMinNeighbors->setValue(2); + _ui->groupBox_assemble->setChecked(true); _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01); @@ -263,11 +279,23 @@ double ExportCloudsDialog::getGenerateMaxDepth() const return _ui->doubleSpinBox_maxDepth->value(); } +bool ExportCloudsDialog::getFiltering() const +{ + return _ui->groupBox_filtering->isChecked(); +} +double ExportCloudsDialog::getFilteringRadius() const +{ + return _ui->doubleSpinBox_filteringRadius->value(); +} +int ExportCloudsDialog::getFilteringMinNeighbors() const +{ + return _ui->spinBox_filteringMinNeighbors->value(); +} + bool ExportCloudsDialog::getAssemble() const { return _ui->groupBox_assemble->isChecked(); } - double ExportCloudsDialog::getAssembleVoxel() const { return _ui->doubleSpinBox_voxelSize_assembled->value(); diff --git a/guilib/src/ExportCloudsDialog.h b/guilib/src/ExportCloudsDialog.h index c27806ed..f74dfdae 100644 --- a/guilib/src/ExportCloudsDialog.h +++ b/guilib/src/ExportCloudsDialog.h @@ -61,6 +61,10 @@ public: double getGenerateVoxel() const; double getGenerateMaxDepth() const; + bool getFiltering() const; + double getFilteringRadius() const; + int getFilteringMinNeighbors() const; + bool getAssemble() const; double getAssembleVoxel() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 0afe18ac..33be7822 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -4619,7 +4619,9 @@ bool MainWindow::getExportedClouds( _exportDialog->getGenerate(), _exportDialog->getGenerateDecimation(), _exportDialog->getGenerateVoxel(), - _exportDialog->getGenerateMaxDepth()); + _exportDialog->getGenerateMaxDepth(), + _exportDialog->getFiltering()?_exportDialog->getFilteringRadius():0.0f, + _exportDialog->getFiltering()?_exportDialog->getFilteringMinNeighbors():0.0f); if(_exportDialog->getAssemble()) { @@ -4642,14 +4644,29 @@ bool MainWindow::getExportedClouds( _initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...") .arg(assembledCloud->size()) - .arg(_exportDialog->getGenerateVoxel())); + .arg(_exportDialog->getAssembleVoxel())); QApplication::processEvents(); - if(_exportDialog->getGenerateVoxel()) + if(_exportDialog->getAssembleVoxel()) { assembledCloud = util3d::voxelize( assembledCloud, - _exportDialog->getGenerateVoxel()); + _exportDialog->getAssembleVoxel()); } + + _initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...") + .arg(assembledCloud->size()) + .arg(_exportDialog->getFilteringRadius()) + .arg(_exportDialog->getFilteringMinNeighbors())); + if(_exportDialog->getFiltering() && + _exportDialog->getFilteringRadius() > 0.0 && + _exportDialog->getFilteringMinNeighbors() > 0) + { + pcl::IndicesPtr indices = util3d::radiusFiltering(assembledCloud, (float)_exportDialog->getFilteringRadius(), _exportDialog->getFilteringMinNeighbors()); + pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud); + pcl::copyPointCloud(*assembledCloud, *indices, *cloudFiltered); + assembledCloud = cloudFiltered; + } + clouds.clear(); clouds.insert(std::make_pair(0, assembledCloud)); } @@ -5488,7 +5505,9 @@ std::map::Ptr > MainWindow::getClouds( bool regenerateClouds, int regenerateDecimation, float regenerateVoxelSize, - float regenerateMaxDepth) const + float regenerateMaxDepth, + float filteringRadius, + float filteringMinNeighbors) const { std::map::Ptr> clouds; int i=0; @@ -5542,6 +5561,14 @@ std::map::Ptr > MainWindow::getClouds( if(cloud->size()) { + if(filteringRadius > 0.0f && filteringMinNeighbors > 0) + { + pcl::IndicesPtr indices = util3d::radiusFiltering(cloud, filteringRadius, filteringMinNeighbors); + pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud); + pcl::copyPointCloud(*cloud, *indices, *cloudFiltered); + cloud = cloudFiltered; + } + clouds.insert(std::make_pair(iter->first, cloud)); inserted = true; } diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 96986185..ad778d1e 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -6,8 +6,8 @@ 0 0 - 807 - 847 + 814 + 898 @@ -23,9 +23,9 @@ 0 - -178 - 766 - 949 + 0 + 773 + 1064 @@ -166,6 +166,73 @@ + + + + Cloud filtering (remove noisy points) + + + true + + + + + + Radius search. + + + true + + + + + + + m + + + 3 + + + 0.001000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.020000000000000 + + + + + + + 1 + + + 100 + + + 2 + + + + + + + Minimum neighbors in the search radius. + + + true + + + + + +