Skip to content

Commit

Permalink
Voxel filter: added support for very large clouds. Gui/Export: added …
Browse files Browse the repository at this point in the history 
…footprint filtering option. Moved all filtering options under Cloud Filtering group.
  • Loading branch information
@matlabbe
matlabbe committed May 15, 2021
1 parent 4390cb6 commit 8c336e1
Show file tree
Hide file tree
Showing 3 changed files with 369 additions and 170 deletions.
76 changes: 68 additions & 8 deletions corelib/src/util3d_filtering.cpp
View file
Expand Up @@ -494,20 +494,80 @@ template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr voxelizeImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize)
float voxelSize,
int level = 0)
{
UASSERT(voxelSize > 0.0f);
typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && !indices->empty()))
{
pcl::VoxelGrid<PointT> 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<PointT>(*cloud, min_p, max_p);
else
pcl::getMinMax3D<PointT>(*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<std::int64_t>((max_p[0] - min_p[0]) * inverseVoxelSize)+1;
std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverseVoxelSize)+1;
std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverseVoxelSize)+1;

if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::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<int>(cloud->size()));
for(size_t i=0; i<cloud->size(); ++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; z<zMax; ++z)
{
Eigen::Vector4f m = min_p+Eigen::Vector4f(mid[0]*x,mid[1]*y,mid[2]*z,0);
Eigen::Vector4f mx = m+mid;
if(zMax==1)
{
mx[2] = max_p[2];
}
pcl::IndicesPtr ind = util3d::cropBox(cloud, denseIndices.get()?denseIndices:indices, m, mx);
if(!ind->empty())
{
// extract indices to avoid high memory usage
*output+=*voxelizeImpl<PointT>(cloud, ind, voxelSize, level+1);
}
}
}
}
}
else
{
pcl::VoxelGrid<PointT> 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)
{
Expand Down
75 changes: 59 additions & 16 deletions guilib/src/ExportCloudsDialog.cpp
View file
Expand Up @@ -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()));
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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());
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -3572,18 +3584,6 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::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<max?min:std::numeric_limits<int>::min(),
max!=0.0f?max:std::numeric_limits<int>::max());
}
}
else if(_ui->checkBox_fromDepth->isChecked() && uContains(cachedClouds, iter->first))
{
Expand Down Expand Up @@ -3689,15 +3689,58 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::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<max?min:std::numeric_limits<int>::min(),
max!=0.0f?max:std::numeric_limits<int>::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
Expand Down

0 comments on commit 8c336e1

Please sign in to comment.