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

added subtractFiltering() method to filter point clouds by subtracting the previous cloud

Browse Source
This commit is contained in:
matlabbe
2015-06-29 00:19:45 -04:00
parent 5269649661
commit fce1816c21
10 changed files with 341 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -206,7 +206,7 @@ signals:
private:
void update3DMapVisibility(bool cloudsShown, bool scansShown);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints, const std::map<int, int> & mapIds, bool verboseProgress = false);
void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
void createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);

2
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -157,8 +157,10 @@ public:
double getMeshSmoothingRadius() const;
bool isCloudFiltering() const;
bool isSubtractFiltering() const;
double getCloudFilteringRadius() const;
double getCloudFilteringAngle() const;
int getSubstractFilteringMinPts() const;
bool getGridMapShown() const;
double getGridMapResolution() const;

7
guilib/src/CloudViewer.cpp
View File

@@ -94,7 +94,6 @@ public:
CloudViewer::CloudViewer(QWidget *parent) :
QVTKWidget(parent),
_visualizer(new pcl::visualization::PCLVisualizer("PCLVisualizer", false)),
_aLockCamera(0),
_aFollowCamera(0),
_aResetCamera(0),
@@ -119,13 +118,15 @@ CloudViewer::CloudViewer(QWidget *parent) :
{
this->setMinimumSize(200, 200);
int argc = 0;
_visualizer = new pcl::visualization::PCLVisualizer(argc, 0, "PCLVisualizer", vtkSmartPointer<MyInteractorStyle>(new MyInteractorStyle()), false);
this->SetRenderWindow(_visualizer->getRenderWindow());
// Replaced by the second line, to avoid a crash in Mac OS X on close, as well as
// the "Invalid drawable" warning when the view is not visible.
//_visualizer->setupInteractor(this->GetInteractor(), this->GetRenderWindow());
vtkSmartPointer<MyInteractorStyle> interactor(new MyInteractorStyle());
this->GetInteractor()->SetInteractorStyle (interactor);
this->GetInteractor()->SetInteractorStyle (_visualizer->getInteractorStyle());
_visualizer->setCameraPosition(
-1, 0, 0,

66
guilib/src/MainWindow.cpp
View File

@@ -1645,6 +1645,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
_preferencesDialog->getCloudDecimation(0),
_preferencesDialog->getCloudMaxDepth(0),
_preferencesDialog->getCloudVoxelSize(0));
_createdClouds.insert(std::make_pair(nodeId, cloud));
if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
{
@@ -1654,7 +1655,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
int minClusterSize = 20;
cv::Mat ground, obstacles;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = cloud;
if(voxelizedCloud->size())
if(voxelizedCloud->size() && cellSize > _preferencesDialog->getCloudVoxelSize(0))
{
voxelizedCloud = util3d::voxelize(cloud, cellSize);
}
@@ -1671,19 +1672,60 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFiltered = cloud;
if(_preferencesDialog->isSubtractFiltering() &&
_preferencesDialog->getCloudVoxelSize(0) > 0.0 &&
cloud->size() &&
_createdClouds.size() &&
_currentPosesMap.size() &&
_currentLinksMap.size())
{
// find link to previous neighbor
std::map<int, Transform>::const_iterator previousIter = _currentPosesMap.find(nodeId);
Link link;
if(previousIter != _currentPosesMap.begin())
{
--previousIter;
std::multimap<int, Link>::const_iterator linkIter = graph::findLink(_currentLinksMap, nodeId, previousIter->first);
if(linkIter != _currentLinksMap.end())
{
link = linkIter->second;
if(link.from() != nodeId)
{
link = link.inverse();
}
}
}
if(link.isValid())
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter = _createdClouds.find(link.to());
if(iter!=_createdClouds.end() && iter->second->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr previousCloud = util3d::transformPointCloud(iter->second, link.transform());
cloudFiltered = util3d::subtractFiltering(
cloud,
previousCloud,
_preferencesDialog->getCloudVoxelSize(0),
_preferencesDialog->getSubstractFilteringMinPts());
UDEBUG("Filtering %d from %d -> %d", (int)previousCloud->size(), (int)cloud->size(), (int)cloudFiltered->size());
}
}
}
if(_preferencesDialog->isCloudMeshing())
{
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
if(cloud->size())
if(cloudFiltered->size())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing())
{
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius());
cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
}
else
{
cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch());
cloudWithNormals = util3d::computeNormals(cloudFiltered, _preferencesDialog->getMeshNormalKSearch());
}
mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius());
}
@@ -1696,10 +1738,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
{
UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
}
else
{
_createdClouds.insert(std::make_pair(nodeId, tmp));
}
}
}
else
@@ -1707,23 +1745,19 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
if(_preferencesDialog->getMeshSmoothing())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius());
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
cloudFiltered.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloudWithNormals, *cloudFiltered);
}
QColor color = Qt::gray;
if(mapId >= 0)
{
color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloudFiltered, pose, color))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}
else
{
_createdClouds.insert(std::make_pair(nodeId, cloud));
}
}
}
else if(iter->getWords3().size())

26
guilib/src/PreferencesDialog.cpp
View File

@@ -295,9 +295,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->checkBox_mls, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->groupBox_poseFiltering, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_nodeFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_subtractFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_cloudFilterRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_cloudFilterAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_substractFilteringMinPts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -999,9 +1001,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_mls->setChecked(false);
_ui->doubleSpinBox_mlsRadius->setValue(0.04);
_ui->groupBox_poseFiltering->setChecked(true);
_ui->checkBox_nodeFiltering->setChecked(true);
_ui->checkBox_subtractFiltering->setChecked(false);
_ui->doubleSpinBox_cloudFilterRadius->setValue(0.1);
_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
_ui->spinBox_substractFilteringMinPts->setValue(0);
_ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_resolution->setValue(0.05);
@@ -1261,9 +1265,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->checkBox_mls->setChecked(settings.value("meshSmoothing", _ui->checkBox_mls->isChecked()).toBool());
_ui->doubleSpinBox_mlsRadius->setValue(settings.value("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value()).toDouble());
_ui->groupBox_poseFiltering->setChecked(settings.value("cloudFiltering", _ui->groupBox_poseFiltering->isChecked()).toBool());
_ui->checkBox_nodeFiltering->setChecked(settings.value("cloudFiltering", _ui->checkBox_nodeFiltering->isChecked()).toBool());
_ui->checkBox_subtractFiltering->setChecked(settings.value("subtractFiltering", _ui->checkBox_subtractFiltering->isChecked()).toBool());
_ui->doubleSpinBox_cloudFilterRadius->setValue(settings.value("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value()).toDouble());
_ui->doubleSpinBox_cloudFilterAngle->setValue(settings.value("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value()).toDouble());
_ui->spinBox_substractFilteringMinPts->setValue(settings.value("cloudFilteringAngleMinPts", _ui->spinBox_substractFilteringMinPts->value()).toDouble());
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
@@ -1551,9 +1557,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("meshSmoothing", _ui->checkBox_mls->isChecked());
settings.setValue("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value());
settings.setValue("cloudFiltering", _ui->groupBox_poseFiltering->isChecked());
settings.setValue("cloudFiltering", _ui->checkBox_nodeFiltering->isChecked());
settings.setValue("subtractFiltering", _ui->checkBox_subtractFiltering->isChecked());
settings.setValue("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value());
settings.setValue("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value());
settings.setValue("subtractFilteringMinPts", _ui->spinBox_substractFilteringMinPts->value());
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
@@ -3069,7 +3077,11 @@ double PreferencesDialog::getMeshSmoothingRadius() const
}
bool PreferencesDialog::isCloudFiltering() const
{
return _ui->groupBox_poseFiltering->isChecked();
return _ui->checkBox_nodeFiltering->isChecked();
}
bool PreferencesDialog::isSubtractFiltering() const
{
return _ui->checkBox_subtractFiltering->isChecked();
}
double PreferencesDialog::getCloudFilteringRadius() const
{
@@ -3079,6 +3091,10 @@ double PreferencesDialog::getCloudFilteringAngle() const
{
return _ui->doubleSpinBox_cloudFilterAngle->value();
}
int PreferencesDialog::getSubstractFilteringMinPts() const
{
return _ui->spinBox_substractFilteringMinPts->value();
}
bool PreferencesDialog::getGridMapShown() const
{
return _ui->checkBox_map_shown->isChecked();

136
guilib/src/ui/preferencesDialog.ui
View File

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>20</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -346,21 +346,21 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
<layout class="QVBoxLayout" name="verticalLayout_31">
<item>
<widget class="QGroupBox" name="groupBox_poseFiltering">
<widget class="QGroupBox" name="groupBox_filtering">
<property name="title">
<string>Cloud filtering</string>
</property>
<property name="checkable">
<bool>true</bool>
<bool>false</bool>
</property>
<property name="checked">
<bool>true</bool>
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_37">
<item>
<widget class="QLabel" name="label_33">
<widget class="QLabel" name="label_160">
<property name="text">
<string>For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept.</string>
<string>For visualization purpose, superposed clouds can be filtered by one or both approaches below.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -368,50 +368,108 @@ Show a yellow background when the number of odometry inliers goes under this thr
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_17" rowstretch="0,0" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<layout class="QGridLayout" name="gridLayout_64" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_37">
<widget class="QLabel" name="label_33">
<property name="text">
<string>Radius.</string>
<string>Node filtering. By comparing poses in the same area, only one cloud in a fixed radius and angle is shown.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
<property name="suffix">
<string> degrees</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<layout class="QGridLayout" name="gridLayout_17" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Radius.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
<property name="suffix">
<string> degrees</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<property name="text">
<string>Angle.</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_nodeFiltering">
<property name="text">
<string>Angle.</string>
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_161">
<property name="text">
<string>Cloud subtraction filtering. When a new cloud is added to the map, the previous cloud is subtracted from the new cloud. Using &quot;Node filtering&quot; at the same time may generate large &quot;holes&quot; in the map (so better to use without &quot;Node filtering&quot;). Voxel size of the map below is used for the radius search of the close points to filter between the two clouds.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_subtractFiltering">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<layout class="QGridLayout" name="gridLayout_65" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_174">
<property name="text">
<string>Minimum number of previous cloud's points in the fixed radius in order to substract the point in the new cloud (radius is the voxel size). Increasing this value reduces the black contours between clouds.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_substractFilteringMinPts"/>
</item>
</layout>
</item>
</layout>
</item>
</layout>