i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

ExportCloudsDialog: added option to filter off-axis points

Browse Source
This commit is contained in:
matlabbe
2024-02-29 16:19:57 -08:00
parent 6f7e5cc97b
commit c24f95a8d9
2 changed files with 416 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
guilib/src/ExportCloudsDialog.cpp
View File

@@ -129,6 +129,14 @@ 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->groupBox_offAxisFiltering, SIGNAL(toggled(bool)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_offAxisFilteringAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringPosX, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringNegX, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringPosY, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringNegY, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringPosZ, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_offAxisFilteringNegZ, SIGNAL(stateChanged(int)), 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()));
@@ -370,6 +378,14 @@ 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_offaxis_filtering", _ui->groupBox_offAxisFiltering->isChecked());
settings.setValue("regenerate_offaxis_filtering_angle", _ui->doubleSpinBox_offAxisFilteringAngle->value());
settings.setValue("regenerate_offaxis_filtering_pos_x", _ui->checkBox_offAxisFilteringPosX->isChecked());
settings.setValue("regenerate_offaxis_filtering_neg_x", _ui->checkBox_offAxisFilteringNegX->isChecked());
settings.setValue("regenerate_offaxis_filtering_pos_y", _ui->checkBox_offAxisFilteringPosY->isChecked());
settings.setValue("regenerate_offaxis_filtering_neg_y", _ui->checkBox_offAxisFilteringNegY->isChecked());
settings.setValue("regenerate_offaxis_filtering_pos_z", _ui->checkBox_offAxisFilteringPosZ->isChecked());
settings.setValue("regenerate_offaxis_filtering_neg_z", _ui->checkBox_offAxisFilteringNegZ->isChecked());
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());
@@ -543,6 +559,14 @@ 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->groupBox_offAxisFiltering->setChecked(settings.value("regenerate_offaxis_filtering", _ui->groupBox_offAxisFiltering->isChecked()).toBool());
_ui->doubleSpinBox_offAxisFilteringAngle->setValue(settings.value("regenerate_offaxis_filtering_angle", _ui->doubleSpinBox_offAxisFilteringAngle->value()).toDouble());
_ui->checkBox_offAxisFilteringPosX->setChecked(settings.value("regenerate_offaxis_filtering_pos_x", _ui->checkBox_offAxisFilteringPosX->isChecked()).toBool());
_ui->checkBox_offAxisFilteringNegX->setChecked(settings.value("regenerate_offaxis_filtering_neg_x", _ui->checkBox_offAxisFilteringNegX->isChecked()).toBool());
_ui->checkBox_offAxisFilteringPosY->setChecked(settings.value("regenerate_offaxis_filtering_pos_y", _ui->checkBox_offAxisFilteringPosY->isChecked()).toBool());
_ui->checkBox_offAxisFilteringNegY->setChecked(settings.value("regenerate_offaxis_filtering_neg_y", _ui->checkBox_offAxisFilteringNegY->isChecked()).toBool());
_ui->checkBox_offAxisFilteringPosZ->setChecked(settings.value("regenerate_offaxis_filtering_pos_z", _ui->checkBox_offAxisFilteringPosZ->isChecked()).toBool());
_ui->checkBox_offAxisFilteringNegZ->setChecked(settings.value("regenerate_offaxis_filtering_neg_z", _ui->checkBox_offAxisFilteringNegZ->isChecked()).toBool());
_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());
@@ -719,6 +743,14 @@ void ExportCloudsDialog::restoreDefaults()
_ui->doubleSpinBox_minDepth->setValue(0);
_ui->doubleSpinBox_ceilingHeight->setValue(0);
_ui->doubleSpinBox_floorHeight->setValue(0);
_ui->groupBox_offAxisFiltering->setChecked(false);
_ui->doubleSpinBox_offAxisFilteringAngle->setValue(10);
_ui->checkBox_offAxisFilteringPosX->setChecked(true);
_ui->checkBox_offAxisFilteringNegX->setChecked(true);
_ui->checkBox_offAxisFilteringPosY->setChecked(true);
_ui->checkBox_offAxisFilteringNegY->setChecked(true);
_ui->checkBox_offAxisFilteringPosZ->setChecked(true);
_ui->checkBox_offAxisFilteringNegZ->setChecked(true);
_ui->doubleSpinBox_footprintHeight->setValue(0);
_ui->doubleSpinBox_footprintLength->setValue(0);
_ui->doubleSpinBox_footprintWidth->setValue(0);
@@ -3915,6 +3947,36 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
UWARN("Point cloud %d doesn't have anymore points (had %d points) after radius filtering.", iter->first, (int)cloud->size());
}
}
if( !indices->empty() && _ui->groupBox_offAxisFiltering->isChecked() &&
(_ui->checkBox_offAxisFilteringPosX->isChecked() ||
_ui->checkBox_offAxisFilteringNegX->isChecked() ||
_ui->checkBox_offAxisFilteringPosY->isChecked() ||
_ui->checkBox_offAxisFilteringNegY->isChecked() ||
_ui->checkBox_offAxisFilteringPosZ->isChecked() ||
_ui->checkBox_offAxisFilteringNegZ->isChecked()))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudInMapFrame = util3d::transformPointCloud(cloud, iter->second);
std::vector<pcl::IndicesPtr> indicesVector;
double maxDeltaAngle = _ui->doubleSpinBox_offAxisFilteringAngle->value()*M_PI/180.0;
Eigen::Vector4f viewpoint(iter->second.x(), iter->second.y(), iter->second.z(), 0);
if(_ui->checkBox_offAxisFilteringPosX->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(1,0,0,0), 20, viewpoint));
if(_ui->checkBox_offAxisFilteringPosY->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,1,0,0), 20, viewpoint));
if(_ui->checkBox_offAxisFilteringPosZ->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,0,1,0), 20, viewpoint));
if(_ui->checkBox_offAxisFilteringNegX->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(-1,0,0,0), 20, viewpoint));
if(_ui->checkBox_offAxisFilteringNegY->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,-1,0,0), 20, viewpoint));
if(_ui->checkBox_offAxisFilteringNegZ->isChecked())
indicesVector.push_back(util3d::normalFiltering(cloudInMapFrame, indices, maxDeltaAngle, Eigen::Vector4f(0,0,-1,0), 20, viewpoint));
indices = util3d::concatenate(indicesVector);
if(indices->empty())
{
UWARN("Point cloud %d doesn't have anymore points (had %d points) after offaxis filtering.", iter->first, (int)cloud->size());
}
}
}
if(!indices->empty())

566
guilib/src/ui/exportCloudsDialog.ui
View File

@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1032</width>
<width>919</width>
<height>869</height>
</rect>
</property>
@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-2140</y>
<width>998</width>
<height>5850</height>
<y>-1328</y>
<width>885</width>
<height>6169</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -1086,220 +1086,362 @@
<property name="checkable">
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_filteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
<layout class="QVBoxLayout" name="verticalLayout_20">
<item>
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_floorHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_filteringRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_filteringMinNeighbors">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>2</number>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Footprint length.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintWidth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_145">
<property name="text">
<string>Footprint height. If negative, the footprint is filtered between [-height:height] around the base frame, otherwise it is [0:height]</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ceilingHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Floor filtering height. This is done in map frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_110">
<property name="text">
<string>Search radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_143">
<property name="text">
<string>Footprint width.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_112">
<property name="text">
<string>Minimum neighbors in the search radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_140">
<property name="text">
<string>Ceiling filtering height. This is done in map frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_110">
<property name="text">
<string>Search radius.</string>
<item>
<widget class="QGroupBox" name="groupBox_offAxisFiltering">
<property name="title">
<string>Off-Axis Filtering</string>
</property>
<property name="wordWrap">
<property name="checkable">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_filteringMinNeighbors">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>2</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_112">
<property name="text">
<string>Minimum neighbors in the search radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ceilingHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_140">
<property name="text">
<string>Ceiling filtering height.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_floorHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_141">
<property name="text">
<string>Floor filtering height.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintWidth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_143">
<property name="text">
<string>Footprint width.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintLength">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Footprint length.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_footprintHeight">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>-1000.000000000000000</double>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_145">
<property name="text">
<string>Footprint height. If negative, the footprint is filtered between [-height:height] around the base frame, otherwise it is [0:height]</string>
</property>
<property name="wordWrap">
<property name="checked">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_21">
<item>
<widget class="QLabel" name="label_148">
<property name="text">
<string>Keep only points with normals aligned with one/any of the map X-Y-Z axes.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_24">
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_offAxisFilteringPosX">
<property name="text">
<string>+X</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QCheckBox" name="checkBox_offAxisFilteringNegY">
<property name="text">
<string>-Y</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QCheckBox" name="checkBox_offAxisFilteringNegX">
<property name="text">
<string>-X</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QCheckBox" name="checkBox_offAxisFilteringPosY">
<property name="text">
<string>+Y</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QCheckBox" name="checkBox_offAxisFilteringPosZ">
<property name="text">
<string>+Z</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="5">
<widget class="QCheckBox" name="checkBox_offAxisFilteringNegZ">
<property name="text">
<string>-Z</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="6">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_offAxisFilteringAngle">
<property name="suffix">
<string> deg</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>179.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>10.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_146">
<property name="text">
<string>Max angle error to keep the points. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>