diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4666661b..53da2f85 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -131,7 +131,6 @@ OPTION(BUILD_LIBS_ONLY "Set to ON to build only the libraries" OFF)
 IF(APPLE)
 	OPTION(BUILD_AS_BUNDLE "Set to ON to build as bundle (DragNDrop)" OFF)
 ENDIF(APPLE)
-OPTION(DEMO_BUILD "Set to ON to build DEMO version" OFF)
 
 ####### DEPENDENCIES #######
 FIND_PACKAGE(OpenCV REQUIRED)
@@ -142,10 +141,6 @@ FIND_PACKAGE(ZLIB REQUIRED)
 # If Qt is here, the GUI will be built
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui QtSvg) 
 
-IF(DEMO_BUILD)
-	ADD_DEFINITIONS(-DDEMO_BUILD)
-ENDIF(DEMO_BUILD)
-
 ####### OSX BUNDLE CMAKE_INSTALL_PREFIX #######
 IF(APPLE AND BUILD_AS_BUNDLE)
   IF(QT4_FOUND AND QT_QTCORE_FOUND AND QT_QTGUI_FOUND)
@@ -334,5 +329,4 @@ MESSAGE(STATUS "  BUILD_LIBS_ONLY = ${BUILD_LIBS_ONLY}")
 IF(APPLE)
 MESSAGE(STATUS "  BUILD_AS_BUNDLE = ${BUILD_AS_BUNDLE}")
 ENDIF(APPLE)
-MESSAGE(STATUS "  DEMO_BUILD = ${DEMO_BUILD}")
 MESSAGE(STATUS "--------------------------------------------")
diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp
index 1c567625..50d4d157 100644
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -41,9 +41,6 @@ Parameters::~Parameters()
 
 std::string Parameters::getDefaultWorkingDirectory()
 {
-#ifdef DEMO_BUILD
-	std::string path = "."; // current directory
-#else
 	std::string path = UDirectory::homeDir();
 	if(!path.empty())
 	{
@@ -55,7 +52,7 @@ std::string Parameters::getDefaultWorkingDirectory()
 	{
 		UFATAL("Can't get the HOME variable environment!");
 	}
-#endif
+
 	path += UDirectory::separator(); // add trailing separator
 	return path;
 }
diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h
index 07a74810..42177c16 100644
--- a/guilib/include/rtabmap/gui/CloudViewer.h
+++ b/guilib/include/rtabmap/gui/CloudViewer.h
@@ -122,8 +122,6 @@ protected:
 
 private:
 	void createMenu();
-	bool frustumCulling(const pcl::PointXYZ & cloud);
-	pcl::IndicesPtr frustumCulling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
 	void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void);
 
 private:
@@ -137,14 +135,12 @@ private:
     QAction * _aClearTrajectory;
     QAction * _aShowGrid;
     QAction * _aSetBackgroundColor;
-    QAction * _setFarPlaneDistance;
     QMenu * _menu;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
-    QMap<std::string, Transform> _addedClouds; // include meshes
+    QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes
     Transform _lastPose;
     std::list<std::string> _gridLines;
-    float _farPlaneDistance;
     QSet<Qt::Key> _keysPressed;
 };
 
diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h
index bb9c25b4..5dd8c4c4 100644
--- a/guilib/include/rtabmap/gui/MainWindow.h
+++ b/guilib/include/rtabmap/gui/MainWindow.h
@@ -170,6 +170,7 @@ signals:
 private:
 	void update3DMapVisibility(bool cloudsShown, bool scansShown);
 	void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
+	std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses) const;
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceImageMenu(int type);
diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h
index 78919e30..82489900 100644
--- a/guilib/include/rtabmap/gui/PreferencesDialog.h
+++ b/guilib/include/rtabmap/gui/PreferencesDialog.h
@@ -125,6 +125,10 @@ public:
 	double getScanOpacity(int index) const;    // 0=map, 1=odom, 2=save
 	int getScanPointSize(int index) const;     // 0=map, 1=odom, 2=save
 
+	bool isCloudFiltering() const;
+	double getCloudFilteringRadius() const;
+	double getCloudFilteringAngle() const;
+
 	QString getWorkingDirectory() const;
 	QString getDatabasePath() const;
 
diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp
index 2d13a7ec..2570e486 100644
--- a/guilib/src/CloudViewer.cpp
+++ b/guilib/src/CloudViewer.cpp
@@ -51,12 +51,10 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aClearTrajectory(0),
 		_aShowGrid(0),
 		_aSetBackgroundColor(0),
-		_setFarPlaneDistance(0),
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
-		_lastPose(Transform::getIdentity()),
-		_farPlaneDistance(10000)
+		_lastPose(Transform::getIdentity())
 {
 	this->setMinimumSize(200, 200);
 
@@ -106,7 +104,6 @@ void CloudViewer::createMenu()
 	_aShowGrid = new QAction("Show grid", this);
 	_aShowGrid->setCheckable(true);
 	_aSetBackgroundColor = new QAction("Set background color...", this);
-	_setFarPlaneDistance = new QAction("Set far plane distance...", this);
 
 	QMenu * cameraMenu = new QMenu("Camera", this);
 	cameraMenu->addAction(_aLockCamera);
@@ -114,7 +111,6 @@ void CloudViewer::createMenu()
 	cameraMenu->addAction(freeCamera);
 	cameraMenu->addSeparator();
 	cameraMenu->addAction(_aLockViewZ);
-	cameraMenu->addAction(_setFarPlaneDistance);
 	cameraMenu->addAction(_aResetCamera);
 	QActionGroup * group = new QActionGroup(this);
 	group->addAction(_aLockCamera);
@@ -483,81 +479,9 @@ void CloudViewer::render()
 
 	}
 
-	// frustum
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(_addedClouds.size());
-	int i=0;
-	for(QMap<std::string, Transform>::iterator iter = _addedClouds.begin(); iter!=_addedClouds.end(); ++iter)
-	{
-		(*cloud)[i++] = pcl::PointXYZ(iter.value().x(), iter.value().y(), iter.value().z());
-	}
-
-	pcl::IndicesPtr indices = frustumCulling(cloud);
-	std::set<int> visibleClouds(indices->begin(), indices->end());
-	i=0;
-	for(QMap<std::string, Transform>::iterator iter = _addedClouds.begin(); iter!=_addedClouds.end(); ++iter)
-	{
-		this->setCloudVisibility(iter.key(), visibleClouds.find(i) != visibleClouds.end());
-		++i;
-	}
-
 	this->GetRenderWindow()->Render();
 }
 
-bool CloudViewer::frustumCulling(const pcl::PointXYZ & point)
-{
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->push_back(point);
-	pcl::IndicesPtr indices = frustumCulling(cloud);
-	return indices->size() != 0;
-}
-
-pcl::IndicesPtr CloudViewer::frustumCulling(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
-{
-	pcl::IndicesPtr indices(new std::vector<int>());
-	if(cloud->size())
-	{
-		if(_farPlaneDistance)
-		{
-			std::vector<pcl::visualization::Camera> cameras;
-			_visualizer->getCameras(cameras);
-
-			Eigen::Vector3f vPosToFocal = Eigen::Vector3f(cameras.front().focal[0] - cameras.front().pos[0],
-														  cameras.front().focal[1] - cameras.front().pos[1],
-														  cameras.front().focal[2] - cameras.front().pos[2]).normalized();
-			Eigen::Vector3f zAxis(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
-			Eigen::Vector3f yAxis = zAxis.cross(vPosToFocal);
-			Eigen::Vector3f xAxis = vPosToFocal.normalized();
-			zAxis = xAxis.cross(yAxis);
-			Transform PR(xAxis[0], yAxis[0], zAxis[0],0,
-						 xAxis[1], yAxis[1], zAxis[1],0,
-						 xAxis[2], yAxis[2], zAxis[2],0);
-			Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
-						PR[4], PR[5], PR[6], cameras.front().pos[1],
-						PR[8], PR[9], PR[10], cameras.front().pos[2]);
-
-			pcl::FrustumCulling<pcl::PointXYZ> fc;
-			fc.setInputCloud (cloud);
-			fc.setNearPlaneDistance (0.0);
-			fc.setFarPlaneDistance (_farPlaneDistance);
-			//float fov = cameras.front().fovy*180.0f/3.14159265359;
-			fc.setVerticalFOV (52);
-			fc.setHorizontalFOV (52);
-			fc.setCameraPose (util3d::transformToEigen4f(P));
-			fc.filter (*indices);
-		}
-		else
-		{
-			indices->resize(cloud->size());
-			for(unsigned int i=0; i<indices->size(); ++i)
-			{
-				indices->at(i) = i;
-			}
-		}
-	}
-	return indices;
-}
-
 void CloudViewer::setBackgroundColor(const QColor & color)
 {
 	_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
@@ -612,7 +536,40 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size)
 	}
 }
 
+Eigen::Vector3f rotatePointAroundAxe(
+		const Eigen::Vector3f & point,
+		const Eigen::Vector3f & axis,
+		float angle)
+{
+	Eigen::Vector3f direction = point;
+	Eigen::Vector3f zAxis = axis;
+	float dotProdZ = zAxis.dot(direction);
+	Eigen::Vector3f ptOnZaxis = zAxis * dotProdZ;
+	direction -= ptOnZaxis;
+	Eigen::Vector3f xAxis = direction.normalized();
+	Eigen::Vector3f yAxis = zAxis.cross(xAxis);
 
+	Eigen::Matrix3f newFrame;
+	newFrame << xAxis[0], yAxis[0], zAxis[0],
+				  xAxis[1], yAxis[1], zAxis[1],
+				  xAxis[2], yAxis[2], zAxis[2];
+
+	// transform to axe frame
+	// transpose=inverse for orthogonal matrices
+	Eigen::Vector3f newDirection = newFrame.transpose() * direction;
+
+	// rotate about z
+	float cosTheta = cos(angle);
+	float sinTheta = sin(angle);
+	float magnitude = newDirection.norm();
+	newDirection[0] = ( magnitude * cosTheta );
+	newDirection[1] = ( magnitude * sinTheta );
+
+	// transform back to global frame
+	direction = newFrame * newDirection;
+
+	return direction + ptOnZaxis;
+}
 
 void CloudViewer::keyReleaseEvent(QKeyEvent * event) {
 	if(event->key() == Qt::Key_Up ||
@@ -643,9 +600,11 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 		//update camera position
 		Eigen::Vector3f pos(cameras.front().pos[0], cameras.front().pos[1], _aLockViewZ->isChecked()?0:cameras.front().pos[2]);
 		Eigen::Vector3f focal(cameras.front().focal[0], cameras.front().focal[1], _aLockViewZ->isChecked()?0:cameras.front().focal[2]);
-		Eigen::Vector3f viewUp(0, 0, 1);
+		Eigen::Vector3f viewUp(cameras.front().view[0], cameras.front().view[1], cameras.front().view[2]);
 		Eigen::Vector3f cummulatedDir(0,0,0);
+		Eigen::Vector3f cummulatedFocalDir(0,0,0);
 		float step = 0.2f;
+		float stepRot = 0.02f; // radian
 		if(_keysPressed.contains(Qt::Key_Up))
 		{
 			Eigen::Vector3f dir;
@@ -674,21 +633,43 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
 		}
 		if(_keysPressed.contains(Qt::Key_Right))
 		{
-			Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step; // strafing right
-			cummulatedDir += dir;
+			if(event->modifiers() & Qt::ShiftModifier)
+			{
+				// rotate right
+				Eigen::Vector3f point = (focal-pos);
+				Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, -stepRot);
+				Eigen::Vector3f diff = newPoint - point;
+				cummulatedFocalDir += diff;
+			}
+			else
+			{
+				Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * step; // strafing right
+				cummulatedDir += dir;
+			}
 		}
 		if(_keysPressed.contains(Qt::Key_Left))
 		{
-			Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step; // strafing left
-			cummulatedDir += dir;
+			if(event->modifiers() & Qt::ShiftModifier)
+			{
+				// rotate left
+				Eigen::Vector3f point = (focal-pos);
+				Eigen::Vector3f newPoint = rotatePointAroundAxe(point, viewUp, stepRot);
+				Eigen::Vector3f diff = newPoint - point;
+				cummulatedFocalDir += diff;
+			}
+			else
+			{
+				Eigen::Vector3f dir = ((focal-pos).cross(viewUp)).normalized() * -step; // strafing left
+				cummulatedDir += dir;
+			}
 		}
 
 		cameras.front().pos[0] += cummulatedDir[0];
 		cameras.front().pos[1] += cummulatedDir[1];
 		cameras.front().pos[2] += cummulatedDir[2];
-		cameras.front().focal[0] += cummulatedDir[0];
-		cameras.front().focal[1] += cummulatedDir[1];
-		cameras.front().focal[2] += cummulatedDir[2];
+		cameras.front().focal[0] += cummulatedDir[0] + cummulatedFocalDir[0];
+		cameras.front().focal[1] += cummulatedDir[1] + cummulatedFocalDir[1];
+		cameras.front().focal[2] += cummulatedDir[2] + cummulatedFocalDir[2];
 		_visualizer->setCameraPosition(
 			cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
 			cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
@@ -734,7 +715,6 @@ void CloudViewer::handleAction(QAction * a)
 				-1, 0, 0,
 				0, 0, 0,
 				0, 0, 1);
-		_farPlaneDistance = 10000;
 		this->render();
 	}
 	else if(a == _aShowGrid)
@@ -779,15 +759,6 @@ void CloudViewer::handleAction(QAction * a)
 		color = QColorDialog::getColor(color, this);
 		this->setBackgroundColor(color);
 	}
-	else if(a == _setFarPlaneDistance)
-	{
-		bool ok;
-		double distance = QInputDialog::getDouble(this, tr("Far plane distance"), tr("Clipping plane distance"), _farPlaneDistance, 1, 10000, 0, &ok);
-		if(ok)
-		{
-			_farPlaneDistance = distance;
-		}
-	}
 }
 
 } /* namespace rtabmap */
diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp
index 566fbb07..b32219b7 100644
--- a/guilib/src/GraphViewer.cpp
+++ b/guilib/src/GraphViewer.cpp
@@ -133,7 +133,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_loopClosureColor(Qt::red),
 		_root(0),
 		_nodeRadius(0.1),
-		_linkWidth(0.15),
+		_linkWidth(0),
 		_gridMap(0)
 {
 	this->setScene(new QGraphicsScene(this));
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index 1c878aa3..c3ac06ac 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -71,10 +71,12 @@
 #include "rtabmap/core/util3d.h"
 #include <pcl/visualization/cloud_viewer.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/io/vtk_io.h>
 #include <pcl/filters/filter.h>
+#include <pcl/search/kdtree.h>
 
 #define LOG_FILE_NAME "LogRtabmap.txt"
 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m"
@@ -128,17 +130,6 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->setupUi(this);
 
 	QString title("RTAB-Map: Real-Time Appearance-Based Mapping");
-#if DEMO_BUILD
-	title.append(" [DEMO]");
-	_ui->actionDump_the_prediction_matrix->setVisible(false);
-	_ui->actionDump_the_memory->setVisible(false);
-	_ui->actionGenerate_map->setVisible(false);
-	_ui->actionGenerate_local_map->setVisible(false);
-	_ui->actionPause_on_local_loop_detection->setVisible(false);
-	_ui->menuImage->setEnabled(false);
-	_ui->doubleSpinBox_stats_timeLimit->setEnabled(false);
-	_ui->label_timeLimit->setEnabled(false);
-#endif
 	this->setWindowTitle(title);
 	this->setWindowIconText(tr("RTAB-Map"));
 
@@ -1021,11 +1012,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	_processingStatistics = false;
 }
 
-void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Transform & currentPose)
+void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const Transform & currentPose)
 {
-	if(poses.size())
+	if(posesIn.size())
 	{
-		_currentPosesMap = poses;
+		_currentPosesMap = posesIn;
 		if(_currentPosesMap.size() && !_ui->actionSave_point_cloud->isEnabled())
 		{
 			//enable save cloud action
@@ -1036,6 +1027,17 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & poses, const Tr
 		}
 	}
 
+	// filter duplicated poses
+	std::map<int, Transform> poses;
+	if(_preferencesDialog->isCloudFiltering())
+	{
+		poses = radiusPosesFiltering(posesIn);
+	}
+	else
+	{
+		poses = posesIn;
+	}
+
 	// Map updated! regenerate the assembled cloud, last pose is the new one
 	UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
 	QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
@@ -1227,6 +1229,89 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
 	_ui->widget_cloudViewer->render();
 }
 
+std::map<int, Transform> MainWindow::radiusPosesFiltering(const std::map<int, Transform> & poses) const
+{
+	float radius = _preferencesDialog->getCloudFilteringRadius();
+	float angle = _preferencesDialog->getCloudFilteringAngle()*3.14159265359/180.0; // convert to rad
+	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		cloud->resize(poses.size());
+		int i=0;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+		}
+
+		// radius filtering
+		std::vector<int> names = uKeys(poses);
+		std::vector<Transform> transforms = uValues(poses);
+
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
+		tree->setInputCloud(cloud);
+		std::set<int> indicesChecked;
+		std::set<int> indicesKept;
+
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			// ignore scans
+			if(indicesChecked.find(i) == indicesChecked.end())
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kDistances;
+				tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
+
+				std::set<int> cloudIndices;
+				const Transform & currentT = transforms.at(i);
+				Eigen::Vector3f vA = util3d::transformToEigen3f(currentT).rotation()*Eigen::Vector3f(1,0,0);
+				for(unsigned int j=0; j<kIndices.size(); ++j)
+				{
+					if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
+					{
+						const Transform & checkT = transforms.at(kIndices[j]);
+						// same orientation?
+						Eigen::Vector3f vB = util3d::transformToEigen3f(checkT).rotation()*Eigen::Vector3f(1,0,0);
+						double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+						if(a <= angle)
+						{
+							cloudIndices.insert(kIndices[j]);
+						}
+					}
+				}
+
+				bool firstAdded = false;
+				for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
+				{
+					if(!firstAdded)
+					{
+						indicesKept.insert(*iter);
+						firstAdded = true;
+					}
+					indicesChecked.insert(*iter);
+				}
+			}
+		}
+
+		//pcl::IndicesPtr indicesOut(new std::vector<int>);
+		//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
+		UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
+		//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
+		//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
+
+		std::map<int, Transform> keptPoses;
+		for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
+		{
+			keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
+		}
+
+		return keptPoses;
+	}
+	else
+	{
+		return poses;
+	}
+}
+
 void MainWindow::processRtabmapEventInit(int status, const QString & info)
 {
 	if((RtabmapEventInit::Status)status == RtabmapEventInit::kInitializing)
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index eb5d39b1..3bfee129 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -69,63 +69,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui = new Ui_preferencesDialog();
 	_ui->setupUi(this);
 
-#ifdef DEMO_BUILD
-	_ui->groupBox_sourceImage->setEnabled(false);
-	_ui->general_checkBox_activateRGBD->setEnabled(false);
-	_ui->general_checkBox_activateRGBD_2->setEnabled(false);
-	_ui->label_activateRGBD->setEnabled(false);
-	_ui->label_activateRGBD_2->setEnabled(false);
-	_ui->general_doubleSpinBox_timeThr->setEnabled(false);
-	_ui->general_doubleSpinBox_timeThr_2->setEnabled(false);
-	_ui->label_timeLimit->setEnabled(false);
-	_ui->label_timeLimit_2->setEnabled(false);
-	_ui->doubleSpinBox_similarityThreshold->setEnabled(false);
-	_ui->doubleSpinBox_similarityThreshold_2->setEnabled(false);
-	_ui->label_similarity->setEnabled(false);
-	_ui->label_similarity_2->setEnabled(false);
-	_ui->general_checkBox_publishStats->setEnabled(false);
-	_ui->general_checkBox_publishStats_2->setEnabled(false);
-	_ui->label_publishStat->setEnabled(false);
-	_ui->general_spinBox_memoryThr->setEnabled(false);
-	_ui->label_maxWmSize->setEnabled(false);
-	_ui->groupBox_publishing->setEnabled(false);
-	_ui->groupBox_statistics->setEnabled(false);
-	_ui->general_doubleSpinBox_recentWmRatio->setEnabled(false);
-	_ui->label_ratioRecent->setEnabled(false);
-	_ui->general_spinBox_maxRetrieved->setEnabled(false);
-	_ui->label_retrieved->setEnabled(false);
-	_ui->general_checkBox_RehearsalIdUpdatedToNewOne->setEnabled(false);
-	_ui->label_rehearsalIdUpdate->setEnabled(false);
-	_ui->general_checkBox_keepRawData->setEnabled(false);
-	_ui->label_keepRawData->setEnabled(false);
-	_ui->general_checkBox_keepRehearsedNodes->setEnabled(false);
-	_ui->label_keepRehearsed->setEnabled(false);
-	_ui->checkBox_kp_publishKeypoints->setEnabled(false);
-	_ui->label_publishWords->setEnabled(false);
-	_ui->checkBox_dictionary_incremental->setEnabled(false);
-	_ui->label_incrementalDict->setEnabled(false);
-	_ui->label_dictionaryPath->setEnabled(false);
-	_ui->lineEdit_dictionaryPath->setEnabled(false);
-	_ui->toolButton_dictionaryPath->setEnabled(false);
-	_ui->groupBox_vh_strategy1->setEnabled(false);
-	_ui->odomScanHistory->setEnabled(false);
-	_ui->label_scanMatching->setEnabled(false);
-	_ui->localDetection_maxNeighbors->setEnabled(false);
-	_ui->localDetection_space->setEnabled(false);
-	_ui->localDetection_radius->setEnabled(false);
-	_ui->label_space1->setEnabled(false);
-	_ui->label_space2->setEnabled(false);
-	_ui->label_space3->setEnabled(false);
-	_ui->loopClosure_icpType->setEnabled(false);
-	_ui->surf_checkBox_gpuVersion->setEnabled(false);
-	_ui->label_surf_checkBox_gpuVersion->setEnabled(false);
-#else
 	if(cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
 		_ui->surf_checkBox_gpuVersion->setEnabled(false);
 		_ui->label_surf_checkBox_gpuVersion->setEnabled(false);
 	}
-#endif
+
 	_ui->predictionPlot->showLegend(false);
 
 	QButtonGroup * buttonGroup = new QButtonGroup(this);
@@ -217,6 +166,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		}
 	}
 
+	connect(_ui->groupBox_poseFiltering, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_cloudFilterRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_cloudFilterAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+
 	//Logging panel
 	connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
 	connect(_ui->comboBox_loggerEventLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -748,6 +701,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 				_3dRenderingMeshing[i]->setChecked(false);
 			}
 		}
+
+		_ui->groupBox_poseFiltering->setChecked(false);
+		_ui->doubleSpinBox_cloudFilterRadius->setValue(0.5);
+		_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
 	{
@@ -857,15 +814,11 @@ QString PreferencesDialog::getDatabasePath() const
 
 QString PreferencesDialog::getIniFilePath() const
 {
-#ifdef DEMO_BUILD
-	QString privatePath = ".";
-#else
 	QString privatePath = QDir::homePath() + "/.rtabmap";
 	if(!QDir(privatePath).exists())
 	{
 		QDir::home().mkdir(".rtabmap");
 	}
-#endif
 	return privatePath + "/rtabmap.ini";
 }
 
@@ -961,6 +914,10 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 		}
 	}
 
+	_ui->groupBox_poseFiltering->setChecked(settings.value("cloudFiltering", _ui->groupBox_poseFiltering->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());
+
 	settings.endGroup(); // General
 
 	settings.endGroup(); // rtabmap
@@ -1159,6 +1116,9 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 			settings.setValue(tr("meshing%1").arg(i), _3dRenderingMeshing[i]->isChecked());
 		}
 	}
+	settings.setValue("cloudFiltering", _ui->groupBox_poseFiltering->isChecked());
+	settings.setValue("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value());
+	settings.setValue("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value());
 
 	settings.endGroup(); // General
 
@@ -1325,11 +1285,11 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->lineEdit_workingDirectory->setEnabled(true);
 		_ui->toolButton_workingDirectory->setEnabled(true);
 		_ui->label_workingDirectory->setEnabled(true);
-#ifndef DEMO_BUILD
+
 		_ui->lineEdit_dictionaryPath->setEnabled(true);
 		_ui->toolButton_dictionaryPath->setEnabled(true);
 		_ui->label_dictionaryPath->setEnabled(true);
-#endif
+
 		_ui->groupBox_source0->setEnabled(true);
 		_ui->groupBox_odometry2->setEnabled(true);
 
@@ -2375,6 +2335,18 @@ int PreferencesDialog::getScanPointSize(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingPtSizeScan[index]->value();
 }
+bool PreferencesDialog::isCloudFiltering() const
+{
+	return _ui->groupBox_poseFiltering->isChecked();
+}
+double PreferencesDialog::getCloudFilteringRadius() const
+{
+	return _ui->doubleSpinBox_cloudFilterRadius->value();
+}
+double PreferencesDialog::getCloudFilteringAngle() const
+{
+	return _ui->doubleSpinBox_cloudFilterAngle->value();
+}
 
 // Source
 double PreferencesDialog::getGeneralInputRate() const
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index f8b5b2db..9273d73e 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -64,8 +64,8 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>761</width>
-            <height>867</height>
+            <width>777</width>
+            <height>826</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>12</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -310,474 +310,552 @@
                    <property name="title">
                     <string>3D Rendering</string>
                    </property>
-                   <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,1">
-                    <item row="5" column="3">
-                     <widget class="QLabel" name="label_155">
-                      <property name="text">
-                       <string>3D cloud opacity.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="1">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
-                      <property name="suffix">
-                       <string> m</string>
-                      </property>
-                      <property name="decimals">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <double>100.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="0">
-                     <widget class="QCheckBox" name="checkBox_showClouds">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="0" column="1">
-                     <widget class="QLabel" name="label_137">
-                      <property name="text">
-                       <string>Odometry</string>
-                      </property>
-                      <property name="alignment">
-                       <set>Qt::AlignCenter</set>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="0">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
-                      <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="3" column="3">
-                     <widget class="QLabel" name="label_108">
-                      <property name="text">
-                       <string>3D cloud decimation (1-2-4-8-...).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="3">
-                     <widget class="QLabel" name="label_132">
-                      <property name="text">
-                       <string>3D cloud maximum depth (0 means no limit).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="0" column="0">
-                     <widget class="QLabel" name="label_154">
-                      <property name="text">
-                       <string>Map</string>
-                      </property>
-                      <property name="alignment">
-                       <set>Qt::AlignCenter</set>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="5" column="0">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
-                      <property name="suffix">
-                       <string/>
-                      </property>
-                      <property name="decimals">
-                       <number>2</number>
-                      </property>
-                      <property name="maximum">
-                       <double>1.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>0.600000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="1" column="3">
-                     <widget class="QLabel" name="label_119">
-                      <property name="text">
-                       <string>Show 3D clouds.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="3" column="0">
-                     <widget class="QSpinBox" name="spinBox_decimation">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>32</number>
-                      </property>
-                      <property name="value">
-                       <number>4</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="1">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_odom">
-                      <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="3" column="1">
-                     <widget class="QSpinBox" name="spinBox_decimation_odom">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>32</number>
-                      </property>
-                      <property name="value">
-                       <number>2</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="0">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
-                      <property name="suffix">
-                       <string> m</string>
-                      </property>
-                      <property name="decimals">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <double>100.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>4.000000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="3">
-                     <widget class="QLabel" name="label_107">
-                      <property name="text">
-                       <string>3D cloud voxel size.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="5" column="1">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
-                      <property name="suffix">
-                       <string/>
-                      </property>
-                      <property name="decimals">
-                       <number>2</number>
-                      </property>
-                      <property name="maximum">
-                       <double>1.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>1.000000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="8" column="3">
-                     <widget class="QLabel" name="label_156">
-                      <property name="text">
-                       <string>2D scan opacity.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="8" column="1">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
-                      <property name="suffix">
-                       <string/>
-                      </property>
-                      <property name="decimals">
-                       <number>2</number>
-                      </property>
-                      <property name="maximum">
-                       <double>1.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>1.000000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="8" column="0">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
-                      <property name="suffix">
-                       <string/>
-                      </property>
-                      <property name="decimals">
-                       <number>2</number>
-                      </property>
-                      <property name="maximum">
-                       <double>1.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>1.000000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="7" column="0">
-                     <widget class="QCheckBox" name="checkBox_showScans">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="7" column="3">
-                     <widget class="QLabel" name="label_110">
-                      <property name="text">
-                       <string>Show 2D scans.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="7" column="1">
-                     <widget class="QCheckBox" name="checkBox_showOdomScans">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="1" column="1">
-                     <widget class="QCheckBox" name="checkBox_showOdomClouds">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="6" column="3">
-                     <widget class="QLabel" name="label_157">
-                      <property name="text">
-                       <string>3D cloud point size (1..64).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="9" column="3">
-                     <widget class="QLabel" name="label_158">
-                      <property name="text">
-                       <string>2D scan point size (1..64).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="0" column="2">
-                     <widget class="QLabel" name="label_159">
-                      <property name="text">
-                       <string>Saving/
+                   <layout class="QVBoxLayout" name="verticalLayout_31">
+                    <item>
+                     <widget class="QFrame" name="frame">
+                      <property name="frameShape">
+                       <enum>QFrame::StyledPanel</enum>
+                      </property>
+                      <property name="frameShadow">
+                       <enum>QFrame::Raised</enum>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,0,1">
+                       <item row="0" column="0">
+                        <widget class="QLabel" name="label_154">
+                         <property name="text">
+                          <string>Map</string>
+                         </property>
+                         <property name="alignment">
+                          <set>Qt::AlignCenter</set>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_137">
+                         <property name="text">
+                          <string>Odometry</string>
+                         </property>
+                         <property name="alignment">
+                          <set>Qt::AlignCenter</set>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="2">
+                        <widget class="QLabel" name="label_159">
+                         <property name="text">
+                          <string>Saving/
 High-res view</string>
+                         </property>
+                         <property name="alignment">
+                          <set>Qt::AlignCenter</set>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="checkBox_showClouds">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QCheckBox" name="checkBox_showOdomClouds">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="2">
+                        <widget class="QCheckBox" name="checkBox_showSaveClouds">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="3">
+                        <widget class="QLabel" name="label_119">
+                         <property name="text">
+                          <string>Show 3D clouds.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
+                         <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="2" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_odom">
+                         <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="2" column="2">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_save">
+                         <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.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="3">
+                        <widget class="QLabel" name="label_107">
+                         <property name="text">
+                          <string>3D cloud voxel size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="spinBox_decimation">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>32</number>
+                         </property>
+                         <property name="value">
+                          <number>4</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QSpinBox" name="spinBox_decimation_odom">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>32</number>
+                         </property>
+                         <property name="value">
+                          <number>2</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="2">
+                        <widget class="QSpinBox" name="spinBox_decimation_save">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>32</number>
+                         </property>
+                         <property name="value">
+                          <number>1</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="3">
+                        <widget class="QLabel" name="label_108">
+                         <property name="text">
+                          <string>3D cloud decimation (1-2-4-8-...).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>100.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>4.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>100.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="2">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_save">
+                         <property name="suffix">
+                          <string> m</string>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <double>100.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>4.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="4" column="3">
+                        <widget class="QLabel" name="label_132">
+                         <property name="text">
+                          <string>3D cloud maximum depth (0 means no limit).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.600000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="5" column="3">
+                        <widget class="QLabel" name="label_155">
+                         <property name="text">
+                          <string>3D cloud opacity.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="0">
+                        <widget class="QSpinBox" name="spinBox_ptsize">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="1">
+                        <widget class="QSpinBox" name="spinBox_ptsize_odom">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="6" column="3">
+                        <widget class="QLabel" name="label_157">
+                         <property name="text">
+                          <string>3D cloud point size (1..64).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QCheckBox" name="checkBox_showScans">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="1">
+                        <widget class="QCheckBox" name="checkBox_showOdomScans">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="2">
+                        <widget class="QCheckBox" name="checkBox_showSaveScans">
+                         <property name="text">
+                          <string/>
+                         </property>
+                         <property name="checked">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="3">
+                        <widget class="QLabel" name="label_110">
+                         <property name="text">
+                          <string>Show 2D scans.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="1">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>2</number>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>1.000000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="8" column="3">
+                        <widget class="QLabel" name="label_156">
+                         <property name="text">
+                          <string>2D scan opacity.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="0">
+                        <widget class="QSpinBox" name="spinBox_ptsize_scan">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="1">
+                        <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>64</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="9" column="3">
+                        <widget class="QLabel" name="label_158">
+                         <property name="text">
+                          <string>2D scan point size (1..64).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="10" column="0">
+                        <widget class="QCheckBox" name="checkBox_meshing">
+                         <property name="text">
+                          <string>Meshing</string>
+                         </property>
+                         <property name="checked">
+                          <bool>false</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_poseFiltering">
+                      <property name="title">
+                       <string>Cloud filtering</string>
                       </property>
-                      <property name="alignment">
-                       <set>Qt::AlignCenter</set>
-                      </property>
-                      <property name="wordWrap">
+                      <property name="checkable">
                        <bool>true</bool>
                       </property>
-                     </widget>
-                    </item>
-                    <item row="1" column="2">
-                     <widget class="QCheckBox" name="checkBox_showSaveClouds">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="2">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_save">
-                      <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.010000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="3" column="2">
-                     <widget class="QSpinBox" name="spinBox_decimation_save">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>32</number>
-                      </property>
-                      <property name="value">
-                       <number>1</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="2">
-                     <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_save">
-                      <property name="suffix">
-                       <string> m</string>
-                      </property>
-                      <property name="decimals">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <double>100.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.100000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>4.000000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="7" column="2">
-                     <widget class="QCheckBox" name="checkBox_showSaveScans">
-                      <property name="text">
-                       <string/>
-                      </property>
-                      <property name="checked">
-                       <bool>true</bool>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="9" column="1">
-                     <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>64</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="9" column="0">
-                     <widget class="QSpinBox" name="spinBox_ptsize_scan">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>64</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="6" column="1">
-                     <widget class="QSpinBox" name="spinBox_ptsize_odom">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>64</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="6" column="0">
-                     <widget class="QSpinBox" name="spinBox_ptsize">
-                      <property name="minimum">
-                       <number>1</number>
-                      </property>
-                      <property name="maximum">
-                       <number>64</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="10" column="0">
-                     <widget class="QCheckBox" name="checkBox_meshing">
-                      <property name="text">
-                       <string>Meshing</string>
-                      </property>
                       <property name="checked">
                        <bool>false</bool>
                       </property>
+                      <layout class="QVBoxLayout" name="verticalLayout_37">
+                       <item>
+                        <widget class="QLabel" name="label_33">
+                         <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>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item>
+                        <layout class="QFormLayout" name="formLayout_22">
+                         <item row="0" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
+                           <property name="minimum">
+                            <double>0.010000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.500000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_37">
+                           <property name="text">
+                            <string>Radius (m)</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_48">
+                           <property name="text">
+                            <string>Angle (degrees)</string>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
+                           <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>
+                        </layout>
+                       </item>
+                      </layout>
                      </widget>
                     </item>
                    </layout>