From 8c336e1e3929e3e1e331b4f7f1b1f151ff70c983 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 14 May 2021 21:02:09 -0400 Subject: [PATCH] Voxel filter: added support for very large clouds. Gui/Export: added footprint filtering option. Moved all filtering options under Cloud Filtering group. --- corelib/src/util3d_filtering.cpp | 76 +++++- guilib/src/ExportCloudsDialog.cpp | 75 ++++-- guilib/src/ui/exportCloudsDialog.ui | 388 +++++++++++++++++----------- 3 files changed, 369 insertions(+), 170 deletions(-) diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp index 25318416e4..d82e852326 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -494,20 +494,80 @@ template typename pcl::PointCloud::Ptr voxelizeImpl( const typename pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, - float voxelSize) + float voxelSize, + int level = 0) { UASSERT(voxelSize > 0.0f); typename pcl::PointCloud::Ptr output(new pcl::PointCloud); - if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())) + if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && !indices->empty())) { - pcl::VoxelGrid filter; - filter.setLeafSize(voxelSize, voxelSize, voxelSize); - filter.setInputCloud(cloud); - if(indices->size()) + Eigen::Vector4f min_p, max_p; + // Get the minimum and maximum dimensions + if(indices->empty()) + pcl::getMinMax3D(*cloud, min_p, max_p); + else + pcl::getMinMax3D(*cloud, *indices, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + float inverseVoxelSize = 1.0f/voxelSize; + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverseVoxelSize)+1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverseVoxelSize)+1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverseVoxelSize)+1; + + if ((dx*dy*dz) > static_cast(std::numeric_limits::max())) { - filter.setIndices(indices); + UWARN("Leaf size is too small for the input dataset. Integer indices would overflow. " + "We will split space to be able to voxelize (lvl=%d cloud=%d min=[%f %f %f] max=[%f %f %f] voxel=%f).", + level, + (int)(indices->empty()?cloud->size():indices->size()), + min_p[0], min_p[1], min_p[2], + max_p[0], max_p[1], max_p[2], + voxelSize); + pcl::IndicesPtr denseIndices; + if(indices->empty()) + { + denseIndices.reset(new std::vector(cloud->size())); + for(size_t i=0; isize(); ++i) + { + denseIndices->at(i) = i; + } + } + + Eigen::Vector4f mid = (max_p-min_p)/2.0f; + int zMax = max_p[2]-min_p[2] < 10?1:2; // do quad tree for 2D maps + for(int x=0; x<2; ++x) + { + for(int y=0; y<2; ++y) + { + for(int z=0; zempty()) + { + // extract indices to avoid high memory usage + *output+=*voxelizeImpl(cloud, ind, voxelSize, level+1); + } + } + } + } + } + else + { + pcl::VoxelGrid filter; + filter.setLeafSize(voxelSize, voxelSize, voxelSize); + filter.setInputCloud(cloud); + if(!indices->empty()) + { + filter.setIndices(indices); + } + filter.filter(*output); } - filter.filter(*output); } else if(cloud->size() && !cloud->is_dense && indices->size() == 0) { diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index bfcc0a3b00..adffb2eb1d 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -126,6 +126,9 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_ceilingHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_floorHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_footprintWidth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_footprintLength, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->doubleSpinBox_footprintHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->spinBox_decimation_scan, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_rangeMin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_rangeMax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -349,6 +352,9 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()); settings.setValue("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()); settings.setValue("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()); + settings.setValue("regenerate_footprint_height", _ui->doubleSpinBox_footprintHeight->value()); + settings.setValue("regenerate_footprint_width", _ui->doubleSpinBox_footprintWidth->value()); + settings.setValue("regenerate_footprint_length", _ui->doubleSpinBox_footprintLength->value()); settings.setValue("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()); settings.setValue("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()); settings.setValue("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()); @@ -507,6 +513,9 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble()); _ui->doubleSpinBox_ceilingHeight->setValue(settings.value("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()).toDouble()); _ui->doubleSpinBox_floorHeight->setValue(settings.value("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()).toDouble()); + _ui->doubleSpinBox_footprintHeight->setValue(settings.value("regenerate_footprint_height", _ui->doubleSpinBox_footprintHeight->value()).toDouble()); + _ui->doubleSpinBox_footprintWidth->setValue(settings.value("regenerate_footprint_width", _ui->doubleSpinBox_footprintWidth->value()).toDouble()); + _ui->doubleSpinBox_footprintLength->setValue(settings.value("regenerate_footprint_length", _ui->doubleSpinBox_footprintLength->value()).toDouble()); _ui->spinBox_decimation_scan->setValue(settings.value("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()).toInt()); _ui->doubleSpinBox_rangeMax->setValue(settings.value("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()).toDouble()); _ui->doubleSpinBox_rangeMin->setValue(settings.value("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()).toDouble()); @@ -668,6 +677,9 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_minDepth->setValue(0); _ui->doubleSpinBox_ceilingHeight->setValue(0); _ui->doubleSpinBox_floorHeight->setValue(0); + _ui->doubleSpinBox_footprintHeight->setValue(0); + _ui->doubleSpinBox_footprintLength->setValue(0); + _ui->doubleSpinBox_footprintWidth->setValue(0); _ui->spinBox_decimation_scan->setValue(1); _ui->doubleSpinBox_rangeMax->setValue(0); _ui->doubleSpinBox_rangeMin->setValue(0); @@ -681,8 +693,8 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_bilateral_sigmaR->setValue(0.1); _ui->checkBox_filtering->setChecked(false); - _ui->doubleSpinBox_filteringRadius->setValue(0.02); - _ui->spinBox_filteringMinNeighbors->setValue(2); + _ui->doubleSpinBox_filteringRadius->setValue(0.00); + _ui->spinBox_filteringMinNeighbors->setValue(5); _ui->checkBox_assemble->setChecked(true); _ui->doubleSpinBox_voxelSize_assembled->setValue(0.01); @@ -3572,18 +3584,6 @@ std::map::Ptr, pcl::Indic UERROR("Cloud %d not found in cache!", iter->first); } } - if(!cloud->empty() && - (_ui->doubleSpinBox_ceilingHeight->value() != 0.0 || _ui->doubleSpinBox_floorHeight->value() != 0.0)) - { - float min = _ui->doubleSpinBox_floorHeight->value(); - float max = _ui->doubleSpinBox_ceilingHeight->value(); - indices = util3d::passThrough( - util3d::transformPointCloud(cloud, iter->second), - indices, - "z", - min!=0.0f&&min::min(), - max!=0.0f?max:std::numeric_limits::max()); - } } else if(_ui->checkBox_fromDepth->isChecked() && uContains(cachedClouds, iter->first)) { @@ -3689,15 +3689,58 @@ std::map::Ptr, pcl::Indic } } - if(indices->size()) + if(_ui->checkBox_filtering->isChecked()) { - if(_ui->checkBox_filtering->isChecked() && + if(!indices->empty() && + (_ui->doubleSpinBox_ceilingHeight->value() != 0.0 || + _ui->doubleSpinBox_floorHeight->value() != 0.0)) + { + float min = _ui->doubleSpinBox_floorHeight->value(); + float max = _ui->doubleSpinBox_ceilingHeight->value(); + indices = util3d::passThrough( + util3d::transformPointCloud(cloud, iter->second), + indices, + "z", + min!=0.0f&&min::min(), + max!=0.0f?max:std::numeric_limits::max()); + } + if(!indices->empty() && + ( _ui->doubleSpinBox_footprintHeight->value() != 0.0 && + _ui->doubleSpinBox_footprintWidth->value() != 0.0 && + _ui->doubleSpinBox_footprintLength->value() != 0.0)) + { + // filter footprint + float h = _ui->doubleSpinBox_footprintHeight->value(); + float w = _ui->doubleSpinBox_footprintWidth->value(); + float l = _ui->doubleSpinBox_footprintLength->value(); + int before= indices->size(); + indices = util3d::cropBox( + cloud, + indices, + Eigen::Vector4f( + -l/2.0f, + -w/2.0f, + h<0.0f?h:0, + 1), + Eigen::Vector4f( + l/2.0f, + w/2.0f, + h<0.0f?-h:h, + 1), + Transform::getIdentity(), + true); + } + + if( !indices->empty() && _ui->doubleSpinBox_filteringRadius->value() > 0.0f && _ui->spinBox_filteringMinNeighbors->value() > 0) { indices = util3d::radiusFiltering(cloud, indices, _ui->doubleSpinBox_filteringRadius->value(), _ui->spinBox_filteringMinNeighbors->value()); } + } + if(!indices->empty()) + { if((_ui->comboBox_frame->isEnabled() && _ui->comboBox_frame->currentIndex()==2) && cloud->isOrganized()) { cloud = util3d::transformPointCloud(cloud, localTransform.inverse()); // put back in camera frame diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index a39ae08583..fe1ef1b422 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - -3906 + -1225 780 - 5635 + 5739 @@ -126,7 +126,7 @@ - Cloud filtering. Remove sparse points that are far from surfaces. + Cloud filtering. true @@ -664,33 +664,61 @@ - - + + - ... + Path to a depth distortion model to apply (output from depth calibration). + + + true - - - - pixels + + + + ROI ratios [left, right, top, bottom] between 0 and 1. Only generate 3D points for pixels inside the region of interest (RGB image). + + + true - - - - -32 + + + + m + + + 2 - 32 + 100.000000000000000 + + + 0.100000000000000 - 1 + 4.000000000000000 + + + + Fill depth hole error. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + @@ -721,36 +749,26 @@ - - - - - - % - - - - - + - 3D cloud maximum depth (0 means no limit). - - - true + - - + + + + + - 3D cloud minimum depth. + Cloud subtraction. Superposed points from different nodes are filtered. true - + Fill depth hole size (0 means no fill). @@ -763,156 +781,74 @@ - - - - m - - - 2 - - - 100.000000000000000 - - - 0.100000000000000 - - - 4.000000000000000 - - - - - + + - Path to a depth distortion model to apply (output from depth calibration). + 3D cloud maximum depth (0 means no limit). true - - - - Fill depth hole error. - - - true - - - Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + -32 - - - - - - ROI ratios [left, right, top, bottom] between 0 and 1. Only generate 3D points for pixels inside the region of interest (RGB image). + + 32 - - true + + 1 - - - - - - - - - - - - - Bilateral filtering of the depth image. Reduce noise in depth images. - - - true + + + % - - + + - Cloud subtraction. Superposed points from different nodes are filtered. - - - true + ... - - + + - - + + - Ceiling filtering height. + 3D cloud minimum depth. true - - + + - Floor filtering height. + Bilateral filtering of the depth image. Reduce noise in depth images. true - - - - m - - - 2 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 - - - - + - m - - - 2 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.100000000000000 - - - 0.000000000000000 + pixels @@ -1114,7 +1050,7 @@ 3 - 0.001000000000000 + 0.000000000000000 1.000000000000000 @@ -1130,7 +1066,7 @@ - Radius search. + Search radius. true @@ -1160,6 +1096,166 @@ + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Ceiling filtering height. + + + true + + + + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Floor filtering height. + + + true + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint width. + + + true + + + + + + + m + + + 2 + + + 0.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint length. + + + true + + + + + + + m + + + 2 + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.100000000000000 + + + 0.000000000000000 + + + + + + + Footprint height. If negative, the footprint is filtered between [-height:height] around the base frame, otherwise it is [0:height] + + + true + + +