From 9f296c67b267ff00266c1fb9642ecc0bba9ba696 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 10 Jun 2016 20:12:13 -0400
Subject: [PATCH] GUI: Added more parameters for 3D projection grid map

---
 .../include/rtabmap/gui/PreferencesDialog.h   |  13 +-
 guilib/src/MainWindow.cpp                     | 254 +++++++-----
 guilib/src/PreferencesDialog.cpp              |  96 ++++-
 guilib/src/ui/DatabaseViewer.ui               | 165 ++++----
 guilib/src/ui/preferencesDialog.ui            | 392 ++++++++++++++----
 5 files changed, 666 insertions(+), 254 deletions(-)

diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
index dca639f3..14a86f0b 100644
--- a/guilib/include/rtabmap/gui/PreferencesDialog.h
+++ b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -147,6 +147,9 @@ public:
 
 	bool isGraphsShown() const;
 	bool isLabelsShown() const;
+	double getMapVoxel() const;
+	double getMapNoiseRadius() const;
+	int getMapNoiseMinNeighbors() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	int getCloudDecimation(int index) const;   // 0=map, 1=odom
 	double getCloudMaxDepth(int index) const;  // 0=map, 1=odom
@@ -172,9 +175,15 @@ public:
 	double getSubtractFilteringAngle() const;
 
 	bool getGridMapShown() const;
-	double getGridMapResolution() const;
-	bool isGridMapFrom3DCloud() const;
+	double getGridMapResolution() const;;
 	bool isGridMapEroded() const;
+	bool isGridMapFrom3DCloud() const;
+	bool projMapFrame() const;
+	double projMaxGroundAngle() const;
+	double projMaxGroundHeight() const;
+	int projMinClusterSize() const;
+	double projMaxObstaclesHeight() const;
+	bool projFlatObstaclesDetected() const;
 	double getGridMapOpacity() const;
 
 	bool isCloudMeshing() const;
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index 0597be3b..1ebd7563 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -1891,10 +1891,14 @@ void MainWindow::updateMapCloud(
 				}
 				else if(_createdClouds.find(iter->first) == _createdClouds.end() && _cachedSignatures.contains(iter->first))
 				{
-					this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
-					if(_createdClouds.find(iter->first) != _createdClouds.end())
+					if((_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
+					    _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end())
 					{
-						_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
+						this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
+						if(_createdClouds.find(iter->first) != _createdClouds.end())
+						{
+							_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
+						}
 					}
 				}
 			}
@@ -2255,135 +2259,185 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 				indices.get(),
 				_preferencesDialog->getAllParameters());
 
+		// filtering pipeline
+		if(indices->size() && _preferencesDialog->getMapVoxel() > 0.0)
+		{
+			cloudWithoutNormals = util3d::voxelize(cloudWithoutNormals, indices, _preferencesDialog->getMapVoxel());
+			//generate indices for all points (they are all valid)
+			indices->resize(cloudWithoutNormals->size());
+			for(unsigned int i=0; i<cloudWithoutNormals->size(); ++i)
+			{
+				indices->at(i) = i;
+			}
+		}
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(indices->size() &&
+		   _preferencesDialog->getMapNoiseRadius() > 0.0 &&
+		   _preferencesDialog->getMapNoiseMinNeighbors() > 0)
+		{
+			indices = rtabmap::util3d::radiusFiltering(
+					cloudWithoutNormals,
+					indices,
+					_preferencesDialog->getMapNoiseRadius(),
+					_preferencesDialog->getMapNoiseMinNeighbors());
+		}
+
 		//compute normals
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::computeNormals(cloudWithoutNormals, 10);
 
-		if(indices->size() && _preferencesDialog->isGridMapFrom3DCloud())
+		if(indices->size() &&
+		  _preferencesDialog->isGridMapFrom3DCloud() &&
+		  _projectionLocalMaps.find(nodeId) == _projectionLocalMaps.end())
 		{
 			UTimer timer;
-			float cellSize = _preferencesDialog->getGridMapResolution();
-			float groundNormalMaxAngle = M_PI_4;
-			int minClusterSize = 20;
 			cv::Mat ground, obstacles;
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = util3d::voxelize(cloudWithoutNormals, indices, cellSize);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloudWithoutNormals;
+
+			// voxelize to grid cell size
+			if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
+			{
+				voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
+			}
 
 			// add pose rotation without yaw
-			float roll, pitch, yaw;
-			pose.getEulerAngles(roll, pitch, yaw);
-			voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0));
+			if(_preferencesDialog->projMapFrame())
+			{
+				float roll, pitch, yaw;
+				pose.getEulerAngles(roll, pitch, yaw);
+				voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0));
+			}
+
+			if(_preferencesDialog->projMaxObstaclesHeight())
+			{
+				voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
+			}
 
 			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
 					voxelCloud,
 					ground,
 					obstacles,
-					cellSize,
-					groundNormalMaxAngle,
-					minClusterSize);
+					_preferencesDialog->getGridMapResolution(),
+					_preferencesDialog->projMaxGroundAngle(),
+					_preferencesDialog->projMinClusterSize(),
+					_preferencesDialog->projFlatObstaclesDetected(),
+					_preferencesDialog->projMaxGroundHeight());
 			if(!ground.empty() || !obstacles.empty())
 			{
 				_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 			}
-			UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
+			UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
 		}
 
-		if(_preferencesDialog->isSubtractFiltering() &&
-					_preferencesDialog->getSubtractFilteringRadius() > 0.0)
+		if(_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0))
 		{
-			pcl::IndicesPtr beforeFiltering = indices;
-			if(	cloud->size() &&
-				_previousCloud.first>0 &&
-				_previousCloud.second.first.get() != 0 &&
-				_previousCloud.second.second.get() != 0 &&
-				_previousCloud.second.second->size() &&
-				_currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
+			if(_preferencesDialog->isSubtractFiltering() &&
+			   _preferencesDialog->getSubtractFilteringRadius() > 0.0)
 			{
-				UTimer time;
+				pcl::IndicesPtr beforeFiltering = indices;
+				if(	cloud->size() &&
+					_previousCloud.first>0 &&
+					_previousCloud.second.first.get() != 0 &&
+					_previousCloud.second.second.get() != 0 &&
+					_previousCloud.second.second->size() &&
+					_currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
+				{
+					UTimer time;
 
-				rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first, t);
+					rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first, t);
 
-				//UWARN("saved new.pcd and old.pcd");
-				//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
-				//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
+					//UWARN("saved new.pcd and old.pcd");
+					//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
+					//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
 
-				indices = rtabmap::util3d::subtractFiltering(
-						cloud,
-						indices,
-						previousCloud,
-						_previousCloud.second.second,
-						_preferencesDialog->getSubtractFilteringRadius(),
-						_preferencesDialog->getSubtractFilteringAngle(),
-						_preferencesDialog->getSubtractFilteringMinPts());
-				UWARN("Time subtract filtering %d from %d -> %d (%fs)",
-						(int)_previousCloud.second.second->size(),
-						(int)beforeFiltering->size(),
-						(int)indices->size(),
-						time.ticks());
+					indices = rtabmap::util3d::subtractFiltering(
+							cloud,
+							indices,
+							previousCloud,
+							_previousCloud.second.second,
+							_preferencesDialog->getSubtractFilteringRadius(),
+							_preferencesDialog->getSubtractFilteringAngle(),
+							_preferencesDialog->getSubtractFilteringMinPts());
+					UWARN("Time subtract filtering %d from %d -> %d (%fs)",
+							(int)_previousCloud.second.second->size(),
+							(int)beforeFiltering->size(),
+							(int)indices->size(),
+							time.ticks());
+				}
+				// keep all indices for next subtraction
+				_previousCloud.first = nodeId;
+				_previousCloud.second.first = cloud;
+				_previousCloud.second.second = beforeFiltering;
 			}
-			// keep all indices for next subtraction
-			_previousCloud.first = nodeId;
-			_previousCloud.second.first = cloud;
-			_previousCloud.second.second = beforeFiltering;
-		}
 
-		// keep substracted clouds
-		_createdClouds.insert(std::make_pair(nodeId, std::make_pair(cloud, indices)));
+			// keep substracted clouds
+			_createdClouds.insert(std::make_pair(nodeId, std::make_pair(cloud, indices)));
 
-		if(indices->size())
-		{
-			if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
+			if(indices->size())
 			{
-				// Fast organized mesh
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output;
-				// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
-				output = util3d::extractIndices(cloud, indices, false, true);
-				Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
-				if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
+				if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized())
 				{
-					viewpoint[0] = data.cameraModels()[0].localTransform().x();
-					viewpoint[1] = data.cameraModels()[0].localTransform().y();
-					viewpoint[2] = data.cameraModels()[0].localTransform().z();
-				}
-				else if(!data.stereoCameraModel().localTransform().isNull())
-				{
-					viewpoint[0] = data.stereoCameraModel().localTransform().x();
-					viewpoint[1] = data.stereoCameraModel().localTransform().y();
-					viewpoint[2] = data.stereoCameraModel().localTransform().z();
-				}
-				std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
-						output,
-						_preferencesDialog->getCloudMeshingAngle(),
-						_preferencesDialog->isCloudMeshingQuad(),
-						_preferencesDialog->getCloudMeshingTriangleSize(),
-						viewpoint);
-				if(polygons.size())
-				{
-					// remove unused vertices to save memory
-					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-					std::vector<pcl::Vertices> outputPolygons;
-					util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
-					if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
+					// Fast organized mesh
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output;
+					// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
+					output = util3d::extractIndices(cloud, indices, false, true);
+					Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
+					if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
 					{
-						UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
+						viewpoint[0] = data.cameraModels()[0].localTransform().x();
+						viewpoint[1] = data.cameraModels()[0].localTransform().y();
+						viewpoint[2] = data.cameraModels()[0].localTransform().z();
+					}
+					else if(!data.stereoCameraModel().localTransform().isNull())
+					{
+						viewpoint[0] = data.stereoCameraModel().localTransform().x();
+						viewpoint[1] = data.stereoCameraModel().localTransform().y();
+						viewpoint[2] = data.stereoCameraModel().localTransform().z();
+					}
+					std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
+							output,
+							_preferencesDialog->getCloudMeshingAngle(),
+							_preferencesDialog->isCloudMeshingQuad(),
+							_preferencesDialog->getCloudMeshingTriangleSize(),
+							viewpoint);
+					if(polygons.size())
+					{
+						// remove unused vertices to save memory
+						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+						std::vector<pcl::Vertices> outputPolygons;
+						util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
+						if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
+						{
+							UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
+						}
+					}
+				}
+				else
+				{
+					if(_preferencesDialog->isCloudMeshing())
+					{
+						UWARN("Online meshing is activated but the generated cloud is "
+							  "dense (voxel filtering is used or multiple cameras are used). Disable "
+							  "online meshing in Preferences->3D Rendering to hide this warning.");
+					}
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output;
+					// don't keep organized to save memory
+					output = util3d::extractIndices(cloud, indices, false, false);
+					QColor color = Qt::gray;
+					if(mapId >= 0)
+					{
+						color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+					}
+
+					if(!_cloudViewer->addCloud(cloudName, output, pose, color))
+					{
+						UERROR("Adding cloud %d to viewer failed!", nodeId);
 					}
 				}
 			}
-			else
-			{
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output;
-				// don't keep organized to save memory
-				output = util3d::extractIndices(cloud, indices, false, false);
-				QColor color = Qt::gray;
-				if(mapId >= 0)
-				{
-					color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
-				}
-
-				if(!_cloudViewer->addCloud(cloudName, output, pose, color))
-				{
-					UERROR("Adding cloud %d to viewer failed!", nodeId);
-				}
-			}
+			_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
+			_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 		}
 	}
 	else
@@ -2391,8 +2445,6 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		return;
 	}
 
-	_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
-	_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 	UDEBUG("");
 }
 
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index e99c1cbd..74872617 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -348,6 +348,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		connect(_3dRenderingPtSizeScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 		connect(_3dRenderingPtSizeFeatures[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	}
+	connect(_ui->doubleSpinBox_voxel, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_noiseRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->spinBox_noiseMinNeighbors, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -364,8 +367,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->groupBox_map_occupancyFrom3DCloud, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_projMapFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_projMaxGroundAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_projMaxGroundHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->spinBox_projMinClusterSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_projMaxObstaclesHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_projFlatObstaclesDetected, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->groupBox_organized, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1167,6 +1176,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingPtSizeScan[i]->setValue(2);
 			_3dRenderingPtSizeFeatures[i]->setValue(3);
 		}
+		_ui->doubleSpinBox_voxel->setValue(0);
+		_ui->doubleSpinBox_noiseRadius->setValue(0);
+		_ui->spinBox_noiseMinNeighbors->setValue(5);
 
 		_ui->checkBox_showGraphs->setChecked(true);
 		_ui->checkBox_showLabels->setChecked(false);
@@ -1182,9 +1194,15 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
-		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
 		_ui->checkBox_map_erode->setChecked(false);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
+		_ui->groupBox_map_occupancyFrom3DCloud->setChecked(false);
+		_ui->checkBox_projMapFrame->setChecked(true);
+		_ui->doubleSpinBox_projMaxGroundAngle->setValue(30);
+		_ui->doubleSpinBox_projMaxGroundHeight->setValue(0);
+		_ui->spinBox_projMinClusterSize->setValue(20);
+		_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(0);
+		_ui->checkBox_projFlatObstaclesDetected->setChecked(true);
 
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
@@ -1512,6 +1530,10 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		_3dRenderingPtSizeScan[i]->setValue(settings.value(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value()).toInt());
 		_3dRenderingPtSizeFeatures[i]->setValue(settings.value(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value()).toInt());
 	}
+	_ui->doubleSpinBox_voxel->setValue(settings.value("cloudVoxel", _ui->doubleSpinBox_voxel->value()).toDouble());
+	_ui->doubleSpinBox_noiseRadius->setValue(settings.value("cloudNoiseRadius", _ui->doubleSpinBox_noiseRadius->value()).toDouble());
+	_ui->spinBox_noiseMinNeighbors->setValue(settings.value("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value()).toInt());
+
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
 
@@ -1526,9 +1548,15 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 
 	_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());
-	_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
 	_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
+	_ui->groupBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()).toBool());
+	_ui->checkBox_projMapFrame->setChecked(settings.value("projMapFrame", _ui->checkBox_projMapFrame->isChecked()).toBool());
+	_ui->doubleSpinBox_projMaxGroundAngle->setValue(settings.value("projMaxGroundAngle", _ui->doubleSpinBox_projMaxGroundAngle->value()).toDouble());
+	_ui->doubleSpinBox_projMaxGroundHeight->setValue(settings.value("projMaxGroundHeight", _ui->doubleSpinBox_projMaxGroundHeight->value()).toDouble());
+	_ui->spinBox_projMinClusterSize->setValue(settings.value("projMinClusterSize", _ui->spinBox_projMinClusterSize->value()).toInt());
+	_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(settings.value("projMaxObstaclesHeight", _ui->doubleSpinBox_projMaxObstaclesHeight->value()).toDouble());
+	_ui->checkBox_projFlatObstaclesDetected->setChecked(settings.value("projFlatObstaclesDetected", _ui->checkBox_projFlatObstaclesDetected->isChecked()).toBool());
 
 	_ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool());
 	_ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble());
@@ -1904,6 +1932,10 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 		settings.setValue(QString("ptSizeScan%1").arg(i), _3dRenderingPtSizeScan[i]->value());
 		settings.setValue(QString("ptSizeFeatures%1").arg(i), _3dRenderingPtSizeFeatures[i]->value());
 	}
+	settings.setValue("cloudVoxel",             _ui->doubleSpinBox_voxel->value());
+	settings.setValue("cloudNoiseRadius",       _ui->doubleSpinBox_noiseRadius->value());
+	settings.setValue("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value());
+
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
 
@@ -1914,14 +1946,22 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("subtractFiltering",       _ui->radioButton_subtractFiltering->isChecked());
 	settings.setValue("subtractFilteringMinPts", _ui->spinBox_subtractFilteringMinPts->value());
 	settings.setValue("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value());
-	settings.setValue("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value());
+	settings.setValue("subtractFilteringAngle",  _ui->doubleSpinBox_subtractFilteringAngle->value());
 
 	settings.setValue("gridMapShown",                _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution",           _ui->doubleSpinBox_map_resolution->value());
-	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
 	settings.setValue("gridMapEroded",               _ui->checkBox_map_erode->isChecked());
 	settings.setValue("gridMapOpacity",              _ui->doubleSpinBox_map_opacity->value());
 
+	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked());
+	settings.setValue("projMapFrame",                _ui->checkBox_projMapFrame->isChecked());
+	settings.setValue("projMaxGroundAngle",          _ui->doubleSpinBox_projMaxGroundAngle->value());
+	settings.setValue("projMaxGroundHeight",         _ui->doubleSpinBox_projMaxGroundHeight->value());
+	settings.setValue("projMinClusterSize",          _ui->spinBox_projMinClusterSize->value());
+	settings.setValue("projMaxObstaclesHeight",      _ui->doubleSpinBox_projMaxObstaclesHeight->value());
+	settings.setValue("projFlatObstaclesDetected",   _ui->checkBox_projFlatObstaclesDetected->isChecked());
+
+
 	settings.setValue("meshing",               _ui->groupBox_organized->isChecked());
 	settings.setValue("meshing_angle",         _ui->doubleSpinBox_mesh_angleTolerance->value());
 	settings.setValue("meshing_quad",          _ui->checkBox_mesh_quad->isChecked());
@@ -3558,6 +3598,20 @@ bool PreferencesDialog::isCloudsShown(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingShowClouds[index]->isChecked();
 }
+
+double PreferencesDialog::getMapVoxel() const
+{
+	return _ui->doubleSpinBox_voxel->value();
+}
+double PreferencesDialog::getMapNoiseRadius() const
+{
+	return _ui->doubleSpinBox_noiseRadius->value();
+}
+int PreferencesDialog::getMapNoiseMinNeighbors() const
+{
+	return _ui->spinBox_noiseMinNeighbors->value();
+}
+
 bool PreferencesDialog::isGraphsShown() const
 {
 	return _ui->checkBox_showGraphs->isChecked();
@@ -3681,14 +3735,38 @@ double PreferencesDialog::getGridMapResolution() const
 {
 	return _ui->doubleSpinBox_map_resolution->value();
 }
-bool PreferencesDialog::isGridMapFrom3DCloud() const
-{
-	return _ui->checkBox_map_occupancyFrom3DCloud->isChecked();
-}
 bool PreferencesDialog::isGridMapEroded() const
 {
 	return _ui->checkBox_map_erode->isChecked();
 }
+bool PreferencesDialog::isGridMapFrom3DCloud() const
+{
+	return _ui->groupBox_map_occupancyFrom3DCloud->isChecked();
+}
+bool PreferencesDialog::projMapFrame() const
+{
+	return _ui->checkBox_projMapFrame->isChecked();
+}
+double PreferencesDialog::projMaxGroundAngle() const
+{
+	return _ui->doubleSpinBox_projMaxGroundAngle->value()*M_PI/180.0;
+}
+double PreferencesDialog::projMaxGroundHeight() const
+{
+	return _ui->doubleSpinBox_projMaxGroundHeight->value();
+}
+int PreferencesDialog::projMinClusterSize() const
+{
+	return _ui->spinBox_projMinClusterSize->value();
+}
+double PreferencesDialog::projMaxObstaclesHeight() const
+{
+	return _ui->doubleSpinBox_projMaxObstaclesHeight->value();
+}
+bool PreferencesDialog::projFlatObstaclesDetected() const
+{
+	return _ui->checkBox_projFlatObstaclesDetected->isChecked();
+}
 double PreferencesDialog::getGridMapOpacity() const
 {
 	return _ui->doubleSpinBox_map_opacity->value();
diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui
index e81065ef..aab22087 100644
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -21,16 +21,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-    <property name="leftMargin">
-     <number>0</number>
-    </property>
-    <property name="topMargin">
-     <number>0</number>
-    </property>
-    <property name="rightMargin">
-     <number>0</number>
-    </property>
-    <property name="bottomMargin">
+    <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -61,7 +52,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>205</width>
+           <width>201</width>
            <height>196</height>
           </rect>
          </property>
@@ -219,7 +210,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>204</width>
+           <width>201</width>
            <height>196</height>
           </rect>
          </property>
@@ -366,16 +357,7 @@
       </item>
       <item row="1" column="0">
        <layout class="QHBoxLayout" name="horizontalLayout">
-        <property name="leftMargin">
-         <number>12</number>
-        </property>
-        <property name="topMargin">
-         <number>12</number>
-        </property>
-        <property name="rightMargin">
-         <number>12</number>
-        </property>
-        <property name="bottomMargin">
+        <property name="margin">
          <number>12</number>
         </property>
         <item>
@@ -431,16 +413,7 @@
       </item>
       <item row="1" column="1">
        <layout class="QHBoxLayout" name="horizontalLayout_2">
-        <property name="leftMargin">
-         <number>12</number>
-        </property>
-        <property name="topMargin">
-         <number>12</number>
-        </property>
-        <property name="rightMargin">
-         <number>12</number>
-        </property>
-        <property name="bottomMargin">
+        <property name="margin">
          <number>12</number>
         </property>
         <item>
@@ -1007,14 +980,14 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>3</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>340</width>
+          <width>333</width>
           <height>186</height>
          </rect>
         </property>
@@ -1149,9 +1122,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
-          <width>282</width>
-          <height>611</height>
+          <y>-236</y>
+          <width>297</width>
+          <height>704</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1225,7 +1198,7 @@
             <bool>true</bool>
            </property>
            <property name="checked">
-            <bool>false</bool>
+            <bool>true</bool>
            </property>
            <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
             <item row="0" column="0">
@@ -1355,6 +1328,81 @@
               </property>
              </widget>
             </item>
+            <item row="8" column="1">
+             <widget class="QLabel" name="label_69">
+              <property name="text">
+               <string>Flat obstacles detected</string>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxGroundHeight">
+              <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>999.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>1.000000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="1">
+             <widget class="QLabel" name="label_68">
+              <property name="text">
+               <string>Max ground height</string>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="1">
+             <widget class="QLabel" name="label_70">
+              <property name="text">
+               <string>Max obstacles height</string>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxObstaclesHeight">
+              <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>999.000000000000000</double>
+              </property>
+              <property name="singleStep">
+               <double>1.000000000000000</double>
+              </property>
+              <property name="value">
+               <double>0.000000000000000</double>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="0">
+             <widget class="QCheckBox" name="checkBox_projFlatObstaclesDetected">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="checked">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
            </layout>
           </widget>
          </item>
@@ -1561,8 +1609,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>205</width>
-          <height>117</height>
+          <width>290</width>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1661,8 +1709,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>267</width>
-          <height>157</height>
+          <width>290</width>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1761,16 +1809,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -1883,16 +1922,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
@@ -1946,16 +1976,7 @@
    </attribute>
    <widget class="QWidget" name="dockWidgetContents_7">
     <layout class="QVBoxLayout" name="verticalLayout_13">
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
       <number>0</number>
      </property>
      <item>
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 283baa0c..58c727b2 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-477</y>
+            <y>-269</y>
             <width>686</width>
             <height>2023</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -629,7 +629,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <item>
                         <widget class="QLabel" name="label_167">
                          <property name="text">
-                          <string>When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated using the laser scans.</string>
+                          <string>When the Graph View is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated using the laser scans or from 3D projection of the clouds (when activated below).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -713,30 +713,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
-                          <widget class="QCheckBox" name="checkBox_map_occupancyFrom3DCloud">
-                           <property name="text">
-                            <string/>
-                           </property>
-                           <property name="checked">
-                            <bool>false</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="1">
-                          <widget class="QLabel" name="label_210">
-                           <property name="text">
-                            <string>Occupancy from 3D cloud projection on the ground. Laser scans are ignored when activated.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="4" column="1">
                           <widget class="QLabel" name="label_224">
                            <property name="text">
                             <string>Erode.</string>
@@ -749,7 +726,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="0">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="checkBox_map_erode">
                            <property name="text">
                             <string/>
@@ -761,6 +738,191 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </item>
                         </layout>
                        </item>
+                       <item>
+                        <widget class="QGroupBox" name="groupBox_map_occupancyFrom3DCloud">
+                         <property name="title">
+                          <string>Occupancy from 3D cloud projection on the ground</string>
+                         </property>
+                         <property name="checkable">
+                          <bool>true</bool>
+                         </property>
+                         <layout class="QVBoxLayout" name="verticalLayout_102">
+                          <item>
+                           <widget class="QLabel" name="label_312">
+                            <property name="text">
+                             <string>Note that 3D cloud filtering stuff under Map columns below are applied before the projection (except for floor and ceiling filterings).</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                          <item>
+                           <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,1">
+                            <item row="0" column="0">
+                             <widget class="QCheckBox" name="checkBox_projMapFrame">
+                              <property name="text">
+                               <string/>
+                              </property>
+                              <property name="checked">
+                               <bool>false</bool>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="1">
+                             <widget class="QLabel" name="label_311">
+                              <property name="text">
+                               <string>Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="1" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxGroundAngle">
+                              <property name="suffix">
+                               <string> deg</string>
+                              </property>
+                              <property name="decimals">
+                               <number>0</number>
+                              </property>
+                              <property name="minimum">
+                               <double>0.000000000000000</double>
+                              </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_307">
+                              <property name="text">
+                               <string>Maximum ground normal angle</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="0">
+                             <widget class="QSpinBox" name="spinBox_projMinClusterSize">
+                              <property name="minimum">
+                               <number>1</number>
+                              </property>
+                              <property name="maximum">
+                               <number>10000</number>
+                              </property>
+                              <property name="value">
+                               <number>20</number>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="1">
+                             <widget class="QLabel" name="label_308">
+                              <property name="text">
+                               <string>Mininum cluster size.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="0">
+                             <widget class="QCheckBox" name="checkBox_projFlatObstaclesDetected">
+                              <property name="text">
+                               <string/>
+                              </property>
+                              <property name="checked">
+                               <bool>false</bool>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="1">
+                             <widget class="QLabel" name="label_272">
+                              <property name="text">
+                               <string>Flat obstacles detected</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="4" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxGroundHeight">
+                              <property name="suffix">
+                               <string> m</string>
+                              </property>
+                              <property name="decimals">
+                               <number>2</number>
+                              </property>
+                              <property name="maximum">
+                               <double>99999.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="4" column="1">
+                             <widget class="QLabel" name="label_310">
+                              <property name="text">
+                               <string>Maximum ground height (0=disabled).</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="5" column="0">
+                             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxObstaclesHeight">
+                              <property name="suffix">
+                               <string> m</string>
+                              </property>
+                              <property name="decimals">
+                               <number>2</number>
+                              </property>
+                              <property name="maximum">
+                               <double>9999.000000000000000</double>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="5" column="1">
+                             <widget class="QLabel" name="label_309">
+                              <property name="text">
+                               <string>Maximum obstacles height (0=disabled).</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                           </layout>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -773,7 +935,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <enum>QFrame::Raised</enum>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
-                       <item row="9" column="0">
+                       <item row="12" column="0">
                         <widget class="QCheckBox" name="checkBox_showScans">
                          <property name="text">
                           <string/>
@@ -783,7 +945,20 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="0">
+                       <item row="12" column="2">
+                        <widget class="QLabel" name="label_110">
+                         <property name="text">
+                          <string>Show scans.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="15" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -802,19 +977,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="2">
-                        <widget class="QLabel" name="label_110">
-                         <property name="text">
-                          <string>Show scans.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="1" column="2">
                         <widget class="QLabel" name="label_119">
                          <property name="text">
@@ -828,7 +990,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="1">
+                       <item row="17" column="1">
                         <widget class="QCheckBox" name="checkBox_showOdomFeatures">
                          <property name="text">
                           <string/>
@@ -838,7 +1000,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="0">
+                       <item row="17" column="0">
                         <widget class="QCheckBox" name="checkBox_showFeatures">
                          <property name="text">
                           <string/>
@@ -912,7 +1074,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="0">
+                       <item row="10" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize">
                          <property name="minimum">
                           <number>1</number>
@@ -925,7 +1087,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="1">
+                       <item row="15" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
                           <string/>
@@ -970,7 +1132,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="2">
+                       <item row="9" column="2">
                         <widget class="QLabel" name="label_155">
                          <property name="text">
                           <string>3D cloud opacity.</string>
@@ -983,7 +1145,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="10" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom">
                          <property name="minimum">
                           <number>1</number>
@@ -996,7 +1158,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="1">
+                       <item row="16" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1006,7 +1168,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="2">
+                       <item row="16" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1019,7 +1181,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="0">
+                       <item row="14" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSizeScan">
                          <property name="suffix">
                           <string> m</string>
@@ -1038,7 +1200,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="2">
+                       <item row="14" column="2">
                         <widget class="QLabel" name="label_271">
                          <property name="text">
                           <string>Scan voxel size.</string>
@@ -1051,7 +1213,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="1">
+                       <item row="13" column="1">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan_odom">
                          <property name="minimum">
                           <number>1</number>
@@ -1061,7 +1223,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="12" column="2">
+                       <item row="15" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1097,7 +1259,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="0">
+                       <item row="9" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
                          <property name="suffix">
                           <string/>
@@ -1116,7 +1278,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="1">
+                       <item row="12" column="1">
                         <widget class="QCheckBox" name="checkBox_showOdomScans">
                          <property name="text">
                           <string/>
@@ -1126,7 +1288,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="16" column="2">
+                       <item row="19" column="2">
                         <widget class="QLabel" name="label_213">
                          <property name="text">
                           <string>Show graphs.</string>
@@ -1139,7 +1301,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="16" column="0">
+                       <item row="19" column="0">
                         <widget class="QCheckBox" name="checkBox_showGraphs">
                          <property name="text">
                           <string/>
@@ -1149,7 +1311,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="6" column="1">
+                       <item row="9" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
                          <property name="suffix">
                           <string/>
@@ -1168,7 +1330,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="13" column="0">
+                       <item row="16" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1188,7 +1350,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="17" column="2">
+                       <item row="20" column="2">
                         <widget class="QLabel" name="label_243">
                          <property name="text">
                           <string>Show labels.</string>
@@ -1201,7 +1363,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="17" column="0">
+                       <item row="20" column="0">
                         <widget class="QCheckBox" name="checkBox_showLabels">
                          <property name="text">
                           <string/>
@@ -1211,7 +1373,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="11" column="1">
+                       <item row="14" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSizeScan_odom">
                          <property name="suffix">
                           <string> m</string>
@@ -1230,7 +1392,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="2">
+                       <item row="13" column="2">
                         <widget class="QLabel" name="label_273">
                          <property name="text">
                           <string>Scan downsampling step size.</string>
@@ -1243,7 +1405,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="10" column="0">
+                       <item row="13" column="0">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan">
                          <property name="minimum">
                           <number>1</number>
@@ -1253,7 +1415,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="2">
+                       <item row="10" column="2">
                         <widget class="QLabel" name="label_157">
                          <property name="text">
                           <string>3D cloud point size (1..64).</string>
@@ -1279,7 +1441,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="14" column="2">
+                       <item row="17" column="2">
                         <widget class="QLabel" name="label_123">
                          <property name="text">
                           <string>Show 3D features.</string>
@@ -1292,7 +1454,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="2">
+                       <item row="18" column="2">
                         <widget class="QLabel" name="label_166">
                          <property name="text">
                           <string>Feature point size.</string>
@@ -1305,7 +1467,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="1">
+                       <item row="18" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_features">
                          <property name="minimum">
                           <number>1</number>
@@ -1315,7 +1477,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="15" column="0">
+                       <item row="18" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_features">
                          <property name="minimum">
                           <number>1</number>
@@ -1379,6 +1541,96 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="6" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxel">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="2">
+                        <widget class="QLabel" name="label_171">
+                         <property name="text">
+                          <string>3D cloud voxel filtering size (0=disabled).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QSpinBox" name="spinBox_noiseMinNeighbors">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>1000</number>
+                         </property>
+                         <property name="value">
+                          <number>5</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="2">
+                        <widget class="QLabel" name="label_168">
+                         <property name="text">
+                          <string>3D cloud noise filtering radius (0=disabled). Done after voxel filtering.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_noiseRadius">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.050000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="2">
+                        <widget class="QLabel" name="label_169">
+                         <property name="text">
+                          <string>3D cloud noise filtering min neighbors.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>