diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h index e0f92bf6..ad8569e5 100644 --- a/corelib/include/rtabmap/core/util3d_filtering.h +++ b/corelib/include/rtabmap/core/util3d_filtering.h @@ -135,6 +135,12 @@ pcl::PointCloud::Ptr RTABMAP_EXP passThrough( float min, float max, bool negative = false); +pcl::PointCloud::Ptr RTABMAP_EXP passThrough( + const pcl::PointCloud::Ptr & cloud, + const std::string & axis, + float min, + float max, + bool negative = false); pcl::IndicesPtr RTABMAP_EXP cropBox( const pcl::PointCloud::Ptr & cloud, diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 80a038a7..1d0dbaeb 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -341,6 +341,26 @@ pcl::PointCloud::Ptr passThrough( return output; } +pcl::PointCloud::Ptr passThrough( + const pcl::PointCloud::Ptr & cloud, + const std::string & axis, + float min, + float max, + bool negative) +{ + UASSERT(max > min); + UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0); + + pcl::PointCloud::Ptr output(new pcl::PointCloud); + pcl::PassThrough filter; + filter.setNegative(negative); + filter.setFilterFieldName(axis); + filter.setFilterLimits(min, max); + filter.setInputCloud(cloud); + filter.filter(*output); + return output; +} + pcl::IndicesPtr cropBox( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 296104f2..dc9a6eea 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -163,6 +163,9 @@ public: double getCeilingFilteringHeight() const; double getFloorFilteringHeight() const; int getNormalKSearch() const; + double getScanCeilingFilteringHeight() const; + double getScanFloorFilteringHeight() const; + int getScanNormalKSearch() const; bool isCloudsShown(int index) const; // 0=map, 1=odom bool isOctomapUpdated() const; bool isOctomapShown() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index c0f8bd0c..1247a4bc 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -2863,6 +2863,24 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m { cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0)); } + + // Do ceiling/floor filtering + if(cloud->size() && + (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || + _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) + { + // perform in /map frame + pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose); + cloudTransformed = rtabmap::util3d::passThrough( + cloudTransformed, + "z", + _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits::min():_preferencesDialog->getScanFloorFilteringHeight(), + _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits::max():_preferencesDialog->getScanCeilingFilteringHeight()); + + //transform back in sensor frame + cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse()); + } + QColor color = Qt::gray; if(mapId >= 0) { @@ -2895,37 +2913,92 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m { pcl::PointCloud::Ptr cloud; cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform()); + bool filtered = false; if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) { cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0)); + filtered = true; } + + // Do ceiling/floor filtering + if(scan.channels() > 2 && // don't filter 2D scans + cloud->size() && + (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || + _preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) + { + // perform in /map frame + pcl::PointCloud::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose); + cloudTransformed = rtabmap::util3d::passThrough( + cloudTransformed, + "z", + _preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits::min():_preferencesDialog->getScanFloorFilteringHeight(), + _preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits::max():_preferencesDialog->getScanCeilingFilteringHeight()); + + //transform back in sensor frame + cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse()); + filtered = true; + } + + pcl::PointCloud::Ptr cloudWithNormals; + if(scan.channels() > 2 && // don't compute normals for 2D scans + cloud->size() && + _preferencesDialog->getScanNormalKSearch() > 0) + { + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch()); + cloudWithNormals.reset(new pcl::PointCloud); + pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); + filtered = true; + } + QColor color = Qt::gray; if(mapId >= 0) { color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); } - if(!_cloudViewer->addCloud(scanName, cloud, pose, color)) + if(cloudWithNormals.get()) { - UERROR("Adding cloud %d to viewer failed!", nodeId); + if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color)) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + else + { + if(nodeId > 0) + { + //reconvert the voxelized cloud + scan = util3d::laserScanFromPointCloud(*cloudWithNormals); + _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame + } + + _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); + _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + } } else { - if(nodeId > 0) + if(!_cloudViewer->addCloud(scanName, cloud, pose, color)) { - if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0) - { - //reconvert the voxelized cloud - scan = util3d::laserScanFromPointCloud(*cloud); - } - else - { - scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform()); - } - _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame + UERROR("Adding cloud %d to viewer failed!", nodeId); } + else + { + if(nodeId > 0) + { + if(filtered) + { + //reconvert the voxelized cloud + scan = util3d::laserScanFromPointCloud(*cloud); + } + else + { + scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform()); + } + _createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame + } - _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); - _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + _cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); + _cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + } } } } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 5f10d715..7a408a90 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -393,6 +393,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_ceilingFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->doubleSpinBox_floorFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->spinBox_normalKSearch_scan, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -1282,13 +1285,16 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_ceilingFilterHeight->setValue(0); _ui->doubleSpinBox_floorFilterHeight->setValue(0); + _ui->spinBox_normalKSearch->setValue(10); + + _ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(0); + _ui->doubleSpinBox_floorFilterHeight_scan->setValue(0); + _ui->spinBox_normalKSearch_scan->setValue(0); _ui->checkBox_showGraphs->setChecked(true); _ui->checkBox_showFrustums->setChecked(false); _ui->checkBox_showLabels->setChecked(false); - _ui->spinBox_normalKSearch->setValue(10); - _ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0); _ui->groupBox_organized->setChecked(false); #if PCL_VERSION_COMPARE(>=, 1, 7, 2) @@ -1674,6 +1680,9 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble()); _ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble()); _ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt()); + _ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(settings.value("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()).toDouble()); + _ui->doubleSpinBox_floorFilterHeight_scan->setValue(settings.value("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()).toDouble()); + _ui->spinBox_normalKSearch_scan->setValue(settings.value("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()).toInt()); _ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool()); _ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool()); @@ -2056,7 +2065,10 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value()); settings.setValue("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()); settings.setValue("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()); - settings.setValue("normalKSearch", _ui->spinBox_normalKSearch->value()); + settings.setValue("normalKSearch", _ui->spinBox_normalKSearch->value()); + settings.setValue("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()); + settings.setValue("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()); + settings.setValue("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()); settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked()); settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked()); @@ -4101,6 +4113,18 @@ int PreferencesDialog::getNormalKSearch() const { return _ui->spinBox_normalKSearch->value(); } +double PreferencesDialog::getScanCeilingFilteringHeight() const +{ + return _ui->doubleSpinBox_ceilingFilterHeight_scan->value(); +} +double PreferencesDialog::getScanFloorFilteringHeight() const +{ + return _ui->doubleSpinBox_floorFilterHeight_scan->value(); +} +int PreferencesDialog::getScanNormalKSearch() const +{ + return _ui->spinBox_normalKSearch_scan->value(); +} bool PreferencesDialog::isGraphsShown() const { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 4a857a71..d5ba5852 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -373 - 673 - 2718 + 0 + 678 + 2701 @@ -86,7 +86,7 @@ QFrame::Raised - 5 + 1 @@ -1084,8 +1084,18 @@ Show a yellow background when the number of odometry inliers goes under this thr Laser Scan - - + + + + + 1 + + + 64 + + + + Scan point size (1..64). @@ -1127,17 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - 1 - - - 64 - - - - + Scan opacity. @@ -1163,7 +1163,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + 1 @@ -1173,6 +1173,16 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + 1 + + + 9999 + + + @@ -1193,17 +1203,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - - - - 1 - - - 9999 - - - - + @@ -1235,7 +1235,7 @@ Show a yellow background when the number of odometry inliers goes under this thr - + @@ -1312,6 +1312,102 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Ceiling filtering height (0=Disabled). This is done in /map frame. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Floor filtering height (0=Disabled). This is done in /map frame. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + m + + + 3 + + + -10.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + m + + + 3 + + + -10.000000000000000 + + + 10.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Normal K search. If not 0, normals will be computed and added to created cloud for visualization (keys 7, 8 and 9). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 0 + + + 1000 + + + 10 + + +