GridMap integration (#1180)

* GridMap integration * Removed GridGlobal/FullUpdate parameter. Bump version 0.21.3. Mvoed specialized global map classes under global_map sub dir. Renamed Map -> GlobalMap. * UI: Added elevation map visualization * Added LocalGridCache class to share cache between global maps * Fixed OctoMap nans. DbViewer: Added frontiers visualization. * convenient functions for ros * Small fix * fixed build without GridMap * CI disabled fail-fast * CI updated checkout action to v4
2026-09-02 17:40:23 +08:00 · 2023-12-17 22:44:11 -08:00
parent 45392fcfc6
commit 71415992ac
44 changed files with 5332 additions and 4255 deletions
--- a/guilib/src/AboutDialog.cpp
+++ b/guilib/src/AboutDialog.cpp
@@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_octomap->setText("No");
 	_ui->label_octomap_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_GRIDMAP
+	_ui->label_gridmap->setText("Yes");
+	_ui->label_gridmap_license->setEnabled(true);
+#else
+	_ui->label_gridmap->setText("No");
+	_ui->label_gridmap_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_CPUTSDF
 	_ui->label_cputsdf->setText("Yes");
 	_ui->label_cputsdf_license->setEnabled(true);
--- a/guilib/src/CloudViewer.cpp
+++ b/guilib/src/CloudViewer.cpp
@@ -53,6 +53,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkCamera.h>
 #include <vtkRenderWindow.h>
 #include <vtkCubeSource.h>
+#include <vtkDataSetMapper.h>
+#include <vtkDelaunay2D.h>
 #include <vtkGlyph3D.h>
 #include <vtkGlyph3DMapper.h>
 #include <vtkSmartVolumeMapper.h>
@@ -67,13 +69,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkPNMReader.h>
 #include <vtkPNGReader.h>
 #include <vtkTIFFReader.h>
+#include <vtkElevationFilter.h>
 #include <vtkOpenGLRenderWindow.h>
 #include <vtkPointPicker.h>
+#include <vtkPointData.h>
 #include <vtkTextActor.h>
 #include <vtkTexture.h>
+#include <vtkNamedColors.h>
 #include <vtkOBBTree.h>
 #include <vtkObjectFactory.h>
 #include <vtkQuad.h>
+#include <vtkWarpScalar.h>
 #include <opencv/vtkImageMatSource.h>

 #if VTK_MAJOR_VERSION >= 7
@@ -87,7 +93,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif

 #ifdef RTABMAP_OCTOMAP
-#include <rtabmap/core/OctoMap.h>
+#include <rtabmap/core/global_map/OctoMap.h>
 #endif

 namespace rtabmap {
@@ -258,6 +264,7 @@ void CloudViewer::clear()
 	this->removeAllTexts();
 	this->removeOccupancyGridMap();
 	this->removeOctomap();
+	this->removeElevationMap();

 	if(_aShowCameraAxis->isChecked())
 	{
@@ -1105,8 +1112,6 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo
 #ifdef RTABMAP_OCTOMAP
 	UASSERT(octomap!=0);

-	pcl::IndicesPtr obstacles(new std::vector<int>);
-
 	if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth())
 	{
 		if(treeDepth>0)
@@ -1122,7 +1127,12 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bo

 	if(!volumeRepresentation)
 	{
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false);
+		pcl::IndicesPtr obstacles(new std::vector<int>);
+		pcl::IndicesPtr ground(new std::vector<int>);
+
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(
+				treeDepth, obstacles.get(), 0, ground.get(), false);
+		obstacles->insert(obstacles->end(), ground->begin(), ground->end());
 		if(obstacles->size())
 		{
 			//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
@@ -1593,6 +1603,126 @@ void CloudViewer::removeOccupancyGridMap()
 #endif
 }

+bool CloudViewer::addElevationMap(
+			const cv::Mat & map32FC1,
+			float resolution, // cell size
+			float xMin,
+			float yMin,
+			float opacity)
+{
+	if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("elevation_map");
+	}
+
+	vtkSmartPointer<vtkPoints> gridPoints = vtkSmartPointer<vtkPoints>::New ();
+	vtkSmartPointer<vtkCellArray> gridCells = vtkSmartPointer<vtkCellArray>::New ();
+	for (int y = 0; y < map32FC1.rows; ++y)
+	{
+		const float * previousRow = y>0?map32FC1.ptr<float>(y-1):0;
+		const float * rowPtr = map32FC1.ptr<float>(y);
+		for (int x = 0; x < map32FC1.cols; ++x)
+		{
+			gridPoints->InsertNextPoint(xMin + x*resolution, yMin + y*resolution, rowPtr[x]);
+			if(x>0 && y>0 &&
+			   rowPtr[x] != 0 &&
+			   rowPtr[x-1] != 0 &&
+			   previousRow[x] != 0 &&
+			   previousRow[x-1] != 0)
+			{
+				gridCells->InsertNextCell(4);
+				gridCells->InsertCellPoint(x-1+y*map32FC1.cols);
+				gridCells->InsertCellPoint(x+y*map32FC1.cols);
+				gridCells->InsertCellPoint(x+(y-1)*map32FC1.cols);
+				gridCells->InsertCellPoint(x-1+(y-1)*map32FC1.cols);
+			}
+		}
+	}
+
+	double bounds[6];
+	gridPoints->GetBounds(bounds);
+
+	vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
+	polyData->SetPoints(gridPoints);
+	polyData->SetPolys(gridCells);
+
+	vtkSmartPointer<vtkElevationFilter> elevationFilter = vtkSmartPointer<vtkElevationFilter>::New ();
+	elevationFilter->SetInputData(polyData);
+	elevationFilter->SetLowPoint(0.0, 0.0, bounds[4]);
+	elevationFilter->SetHighPoint(0.0, 0.0, bounds[5]);
+	elevationFilter->Update();
+
+	vtkSmartPointer<vtkPolyData> output = vtkSmartPointer<vtkPolyData>::New ();
+	output->ShallowCopy(dynamic_cast<vtkPolyData*>(elevationFilter->GetOutput()));
+
+	vtkFloatArray* elevation = dynamic_cast<vtkFloatArray*>(
+	  output->GetPointData()->GetArray("Elevation"));
+
+	// Create the color map
+	vtkSmartPointer<vtkLookupTable> colorLookupTable = vtkSmartPointer<vtkLookupTable>::New ();
+	colorLookupTable->SetTableRange(bounds[4], bounds[5]);
+	colorLookupTable->Build();
+
+	// Generate the colors for each point based on the color map
+	vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
+	colors->SetNumberOfComponents(3);
+	colors->SetName("Colors");
+
+	for (vtkIdType i = 0; i < output->GetNumberOfPoints(); i++)
+	{
+		double val = elevation->GetValue(i);
+		double dcolor[3];
+		colorLookupTable->GetColor(val, dcolor);
+		unsigned char color[3];
+		for (unsigned int j = 0; j < 3; j++)
+		{
+			color[j] = 255 * dcolor[j] / 1.0;
+		}
+
+		colors->InsertNextTypedTuple(color);
+	}
+
+	output->GetPointData()->AddArray(colors);
+
+	vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
+	mapper->SetInputData(output);
+
+	vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
+	actor->SetMapper(mapper);
+
+	// Add it to all renderers
+	_visualizer->getRendererCollection()->InitTraversal ();
+	vtkRenderer* renderer = NULL;
+	int i = 0;
+	int viewport = 1;
+	while ((renderer = _visualizer->getRendererCollection()->GetNextItem ()) != NULL)
+	{
+		// Should we add the actor to all renderers?
+		if (viewport == 0)
+		{
+			renderer->AddActor (actor);
+		}
+		else if (viewport == i)               // add the actor only to the specified viewport
+		{
+			renderer->AddActor (actor);
+		}
+		++i;
+	}
+
+	(*_visualizer->getShapeActorMap())["elevation_map"] = actor;
+
+	setCloudOpacity("elevation_map", opacity);
+
+	return true;
+}
+void CloudViewer::removeElevationMap()
+{
+	if(_visualizer->getShapeActorMap()->find("elevation_map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("elevation_map");
+	}
+}
+
 void CloudViewer::addOrUpdateCoordinate(
 			const std::string & id,
 			const Transform & transform,
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -70,7 +70,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/RegistrationIcp.h"
-#include "rtabmap/core/OccupancyGrid.h"
+#include "rtabmap/core/global_map/OccupancyGrid.h"
+#include "rtabmap/core/global_map/CloudMap.h"
 #include "rtabmap/core/GeodeticCoords.h"
 #include "rtabmap/core/Recovery.h"
 #include "rtabmap/gui/DataRecorder.h"
@@ -92,9 +93,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/crop_box.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
+#include <rtabmap/core/LocalGridMaker.h>

 #ifdef RTABMAP_OCTOMAP
-#include "rtabmap/core/OctoMap.h"
+#include "rtabmap/core/global_map/OctoMap.h"
+#endif
+
+#ifdef RTABMAP_GRIDMAP
+#include "rtabmap/core/global_map/GridMap.h"
 #endif

 namespace rtabmap {
@@ -196,6 +202,13 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->checkBox_octomap->setChecked(false);
 #endif

+#ifndef RTABMAP_GRIDMAP
+	ui_->checkBox_showElevation->setEnabled(false);
+	ui_->checkBox_showElevation->setChecked(false);
+	ui_->checkBox_grid_elevation->setEnabled(false);
+	ui_->checkBox_grid_elevation->setChecked(false);
+#endif
+
 	ParametersMap parameters;
 	uInsert(parameters, Parameters::getDefaultParameters("SURF"));
 	uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
@@ -233,9 +246,11 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+	ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 	ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+	ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);

 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
@@ -346,6 +361,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
+	connect(ui_->checkBox_showElevation, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
 	connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));

@@ -385,10 +401,13 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
+	connect(ui_->checkBox_grid_grid, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
+	connect(ui_->checkBox_grid_elevation, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
+	connect(ui_->checkBox_grid_frontiers, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
@@ -446,12 +465,15 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
 	connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
 	connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
+	connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
 	connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
 	connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
 	connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
+	connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
 	connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
 	connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
 	connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
+	connect(ui_->toolButton_frontierColor, SIGNAL(clicked(bool)), this, SLOT(selectFrontierColor()));
 	connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -568,6 +590,7 @@ void DatabaseViewer::readSettings()
 	ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
 	ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
 	ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
+	ui_->lineEdit_frontierColor->setText(settings.value("colorFrontier", ui_->lineEdit_frontierColor->text()).toString());
 	ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
 	ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool());
 	settings.endGroup();
@@ -657,6 +680,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
 	settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
 	settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
+	settings.setValue("colorFrontier", ui_->lineEdit_frontierColor->text());
 	settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
 	settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked());
 	settings.endGroup();
@@ -744,10 +768,12 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
 	ui_->doubleSpinBox_posefilteringAngle->setValue(30);
 	ui_->checkBox_grid_empty->setChecked(true);
+	ui_->checkBox_grid_frontiers->setChecked(false);
 	ui_->checkBox_octomap->setChecked(false);
 	ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
 	ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
 	ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
+	ui_->lineEdit_frontierColor->setText(QColor(Qt::cyan).name());
 	ui_->spinBox_cropRadius->setValue(1);
 	ui_->checkBox_grid_showProbMap->setChecked(false);

@@ -974,24 +1000,20 @@ bool DatabaseViewer::closeDatabase()

 			if(button == QMessageBox::Yes)
 			{
-				UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
-				std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
-				std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
-				for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
+				for(std::map<int, LocalGrid>::const_iterator mapIter = generatedLocalMaps_.localGrids().begin();
+					mapIter!=generatedLocalMaps_.localGrids().end();
+					++mapIter)
 				{
-					UASSERT(mapIter->first == infoIter->first);
 					dbDriver_->updateOccupancyGrid(
 							mapIter->first,
-							mapIter->second.first.first,
-							mapIter->second.first.second,
-							mapIter->second.second,
-							infoIter->second.first,
-							infoIter->second.second);
+							mapIter->second.groundCells,
+							mapIter->second.obstacleCells,
+							mapIter->second.emptyCells,
+							mapIter->second.cellSize,
+							mapIter->second.viewPoint);
 				}
 				generatedLocalMaps_.clear();
-				generatedLocalMapsInfo_.clear();
 				localMaps_.clear();
-				localMapsInfo_.clear();

 				// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
 				dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
@@ -1049,9 +1071,7 @@ bool DatabaseViewer::closeDatabase()
 		linksRefined_.clear();
 		linksRemoved_.clear();
 		localMaps_.clear();
-		localMapsInfo_.clear();
 		generatedLocalMaps_.clear();
-		generatedLocalMapsInfo_.clear();
 		modifiedLaserScans_.clear();
 		ui_->graphViewer->clearAll();
 		occupancyGridViewer_->clear();
@@ -2332,6 +2352,14 @@ void DatabaseViewer::selectEmptyColor()
 		ui_->lineEdit_emptyColor->setText(c.name());
 	}
 }
+void DatabaseViewer::selectFrontierColor()
+{
+	QColor c = QColorDialog::getColor(ui_->lineEdit_frontierColor->text(), this);
+	if(c.isValid())
+	{
+		ui_->lineEdit_frontierColor->setText(c.name());
+	}
+}
 void DatabaseViewer::editDepthImage()
 {
 	if(dbDriver_ && ids_.size())
@@ -2967,9 +2995,9 @@ void DatabaseViewer::editSaved2DMap()
 				LaserScan scan;
 				data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);

-				if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
+				if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
 				{
-					gridObstacles = generatedLocalMaps_.find(iter->first)->second.first.second;
+					gridObstacles = generatedLocalMaps_.localGrids().at(iter->first).obstacleCells;
 				}
 				if(!gridObstacles.empty())
 				{
@@ -3017,19 +3045,19 @@ void DatabaseViewer::editSaved2DMap()
 						UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);

 						// update
-						std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> value;
-						if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
+						cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
+						if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
 						{
-							value = generatedLocalMaps_.at(iter->first);
+							LocalGrid value = generatedLocalMaps_.localGrids().at(iter->first);
+							value.obstacleCells = newObstacles;
+							generatedLocalMaps_.add(iter->first, value);
 						}
 						else
 						{
-							value.first.first = gridGround;
-							value.second = gridEmpty;
-							uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
+							LocalGrid value(gridGround, newObstacles, gridEmpty, data.gridCellSize(), data.gridViewPoint());
+							generatedLocalMaps_.add(iter->first, value);
 						}
-						value.first.second = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
-						uInsert(generatedLocalMaps_, std::make_pair(iter->first, value));
+
 					}
 				}

@@ -3290,19 +3318,7 @@ void DatabaseViewer::regenerateSavedMap()
 	if(type.compare("From OctoMap projection") == 0)
 	{
 		//create local octomap
-		OctoMap octomap(ui_->parameters_toolbox->getParameters());
-		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
-		{
-			if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
-			{
-				QMessageBox::warning(this, tr(""),
-						tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
-							"occupancy grids. Select default occupancy grid or generate "
-							"3D local occupancy grids (\"Grid/3D\" core parameter)."));
-				return;
-			}
-			octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second);
-		}
+		OctoMap octomap(&localMaps_, ui_->parameters_toolbox->getParameters());

 		octomap.update(graphes_.back());
 		map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
@@ -3310,11 +3326,7 @@ void DatabaseViewer::regenerateSavedMap()
 	else
 #endif
 	{
-		OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
-		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
-		{
-			grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
-		}
+		OccupancyGrid grid(&localMaps_, ui_->parameters_toolbox->getParameters());
 		grid.update(graphes_.back());
 		map = grid.getMap(xMin, yMin);
 	}
@@ -3746,10 +3758,9 @@ void DatabaseViewer::generateLocalGraph()

 void DatabaseViewer::regenerateLocalMaps()
 {
-	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+	LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());

 	generatedLocalMaps_.clear();
-	generatedLocalMapsInfo_.clear();

 	rtabmap::ProgressDialog progressDialog(this);
 	progressDialog.setMaximumSteps(ids_.size());
@@ -3854,17 +3865,16 @@ void DatabaseViewer::regenerateLocalMaps()
 						}
 					}

-					grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
+					localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
 				}
 			}
 			else
 			{
-				grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
+				localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
 			}

 			gridCreationTime = timer.ticks()*1000.0;
-			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty)));
-			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
+			generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
 			msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());

 			totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
@@ -3900,7 +3910,7 @@ void DatabaseViewer::regenerateLocalMaps()
 void DatabaseViewer::regenerateCurrentLocalMaps()
 {
 	UTimer time;
-	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+	LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());

 	if(ids_.size() == 0)
 	{
@@ -3923,9 +3933,6 @@ void DatabaseViewer::regenerateCurrentLocalMaps()

 	for(int i =0; i<ids.size(); ++i)
 	{
-		generatedLocalMaps_.erase(ids.at(i));
-		generatedLocalMapsInfo_.erase(ids.at(i));
-
 		SensorData data;
 		dbDriver_->getNodeData(ids.at(i), data);
 		data.uncompressData();
@@ -3998,17 +4005,15 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 						}
 					}

-					grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
+					localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
 				}
 			}
 			else
 			{
-				grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
+				localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
 			}

-
-			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
-			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
+			generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
 			msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
 		}

@@ -4450,7 +4455,6 @@ void DatabaseViewer::resetAllChanges()
 		linksRefined_.clear();
 		linksRemoved_.clear();
 		generatedLocalMaps_.clear();
-		generatedLocalMapsInfo_.clear();
 		modifiedLaserScans_.clear();
 		updateLoopClosuresSlider();
 		this->updateGraphView();
@@ -4876,9 +4880,10 @@ void DatabaseViewer::update(int value,
 				{
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
-					cloudViewer_->removeCloud("map");
+					cloudViewer_->removeOccupancyGridMap();
 					cloudViewer_->removeAllClouds();
 					cloudViewer_->removeOctomap();
+					cloudViewer_->removeElevationMap();

 					Transform pose = Transform::getIdentity();
 					if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
@@ -5200,21 +5205,20 @@ void DatabaseViewer::update(int value,
 					}

 					//add occupancy grid
-					if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
+					if(ui_->checkBox_showMap->isChecked() ||
+					   ui_->checkBox_showGrid->isChecked() ||
+					   ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
 					{
-						std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
-						std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
-						if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
+						LocalGridCache combinedLocalMaps;
+						if(generatedLocalMaps_.shareTo(data.id(), combinedLocalMaps))
 						{
-							localMaps.insert(*generatedLocalMaps_.find(data.id()));
-							localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
+							// add local grid
 						}
 						else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
 						{
-							localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
-							localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
+							combinedLocalMaps.add(data.id(), data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
 						}
-						if(!localMaps.empty())
+						if(!combinedLocalMaps.empty())
 						{
 							std::map<int, Transform> poses;
 							poses.insert(std::make_pair(data.id(), pose));
@@ -5222,29 +5226,26 @@ void DatabaseViewer::update(int value,
 #ifdef RTABMAP_OCTOMAP
 							OctoMap * octomap = 0;
 							if(ui_->checkBox_octomap->isChecked() &&
-								(!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
-								(localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
-								(localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
-								(localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
-								localMapsInfo.begin()->second.first > 0.0f)
+								(!combinedLocalMaps.localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.localGrids().begin()->second.obstacleCells.empty()) &&
+								combinedLocalMaps.localGrids().begin()->second.is3D() &&
+								combinedLocalMaps.localGrids().begin()->second.cellSize > 0.0f)
 							{
 								//create local octomap
 								ParametersMap params;
-								params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(localMapsInfo.begin()->second.first)));
-								octomap = new OctoMap(params);
-								octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
+								params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(combinedLocalMaps.localGrids().begin()->second.cellSize)));
+								octomap = new OctoMap(&combinedLocalMaps, params);
 								octomap->update(poses);
 							}
 #endif
-
+							ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 							if(ui_->checkBox_showMap->isChecked())
 							{
 								float xMin=0.0f, yMin=0.0f;
 								cv::Mat map8S;
-								ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 								float gridCellSize = Parameters::defaultGridCellSize();
 								Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
 								parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
+
 #ifdef RTABMAP_OCTOMAP
 								if(octomap)
 								{
@@ -5253,9 +5254,7 @@ void DatabaseViewer::update(int value,
 								else
 #endif
 								{
-									OccupancyGrid grid(parameters);
-									grid.setCellSize(gridCellSize);
-									grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
+									OccupancyGrid grid(&combinedLocalMaps, parameters);
 									grid.update(poses);
 									map8S = grid.getMap(xMin, yMin);
 								}
@@ -5320,7 +5319,7 @@ void DatabaseViewer::update(int value,
 #endif
 								{
 									// occupancy cloud
-									LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
+									LaserScan scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.groundCells);
 									if(scan.hasRGB())
 									{
 										cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
@@ -5329,7 +5328,7 @@ void DatabaseViewer::update(int value,
 									{
 										cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
 									}
-									scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.second);
+									scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.obstacleCells);
 									if(scan.hasRGB())
 									{
 										cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
@@ -5345,7 +5344,7 @@ void DatabaseViewer::update(int value,
 									if(ui_->checkBox_grid_empty->isChecked())
 									{
 										cloudViewer_->addCloud("empty_cells",
-												util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)),
+												util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.emptyCells)),
 												pose,
 												QColor(ui_->lineEdit_emptyColor->text()));
 										cloudViewer_->setCloudPointSize("empty_cells", 5);
@@ -5359,6 +5358,34 @@ void DatabaseViewer::update(int value,
 								delete octomap;
 							}
 #endif
+
+#ifdef RTABMAP_GRIDMAP
+							if(ui_->checkBox_showElevation->checkState() != Qt::Unchecked) // Show elevation map?
+							{
+								GridMap gridMap(&combinedLocalMaps, parameters);
+
+								if(combinedLocalMaps.localGrids().begin()->second.is3D())
+								{
+									gridMap.update(poses);
+									if(ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked)
+									{
+										float xMin, yMin, gridCellSize;
+										cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, gridCellSize);
+										cloudViewer_->addElevationMap(elevationMap, gridCellSize, xMin, yMin, 1.0f);
+									}
+									else
+									{
+										pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
+										cloudViewer_->addCloudMesh("elevation_mesh", mesh);
+									}
+									cloudViewer_->refreshView();
+								}
+								else
+								{
+									UWARN("Local grid is not 3D, cannot generate an elevation map");
+								}
+							}
+#endif
 						}
 					}
 					cloudViewer_->updateCameraTargetPosition(pose);
@@ -6781,14 +6808,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					ui_->doubleSpinBox_posefilteringRadius->value(),
 					ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
 		}
-		std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
-		std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
+		LocalGridCache combinedLocalMaps;
 #ifdef RTABMAP_OCTOMAP
-		if(octomap_)
-		{
-			delete octomap_;
-			octomap_ = 0;
-		}
+		delete octomap_;
+		octomap_ = 0;
 #endif
 		if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
 		{
@@ -6797,18 +6820,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			std::vector<int> ids = uKeys(graphFiltered);
 			for(unsigned int i=0; i<ids.size(); ++i)
 			{
-				if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
+				if(generatedLocalMaps_.shareTo(ids[i], combinedLocalMaps) ||
+				   localMaps_.shareTo(ids[i], combinedLocalMaps))
 				{
-					localMaps.insert(*generatedLocalMaps_.find(ids[i]));
-					localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
-				}
-				else if(localMaps_.find(ids[i]) != localMaps_.end())
-				{
-					if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
-					{
-						localMaps.insert(*localMaps_.find(ids.at(i)));
-						localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
-					}
+					// Added to combined maps
 				}
 				else if(ids.at(i)>0)
 				{
@@ -6816,29 +6831,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					dbDriver_->getNodeData(ids.at(i), data, false, false, false);
 					cv::Mat ground, obstacles, empty;
 					data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
-					localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
-					localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
+					localMaps_.add(ids.at(i), ground, obstacles, empty, data.gridCellSize(), data.gridViewPoint());
 					if(!ground.empty() || !obstacles.empty())
 					{
-						localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
-						localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
+						localMaps_.shareTo(ids.at(i), combinedLocalMaps);
 					}
 				}
 			}
-			//cleanup
-			for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
-			{
-				if(graphFiltered.find(iter->first) == graphFiltered.end())
-				{
-					localMapsInfo_.erase(iter->first);
-					localMaps_.erase(iter++);
-				}
-				else
-				{
-					++iter;
-				}
-			}
-			UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
+			UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)combinedLocalMaps.size(), (int)graph.size());
 		}

 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
@@ -6871,7 +6871,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		QGraphicsRectItem * rectScaleItem = 0;
 		ui_->graphViewer->clearMap();
 		occupancyGridViewer_->clear();
-		if(graph.size() && localMaps.size() &&
+		if(graph.size() && combinedLocalMaps.size() &&
 			(ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
 		{
 			QElapsedTimer time;
@@ -6880,28 +6880,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 #ifdef RTABMAP_OCTOMAP
 			if(ui_->checkBox_octomap->isChecked())
 			{
-				octomap_ = new OctoMap(parameters);
-				bool updateAborted = false;
-				for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
-				{
-					if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
-					{
-						QMessageBox::warning(this, tr(""),
-								tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
-									"occupancy grids. Uncheck OctoMap under GUI parameters or generate "
-									"3D local occupancy grids (\"Grid/3D\" core parameter)."));
-						updateAborted = true;
-						break;
-					}
-					octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
-				}
-				if(!updateAborted)
-				{
-					octomap_->update(graphFiltered);
-				}
+				octomap_ = new OctoMap(&combinedLocalMaps, parameters);
+				octomap_->update(graphFiltered);
 			}
 #endif
-
 			// Generate 2d grid map?
 			if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
 			   (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
@@ -6923,12 +6905,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					{
 						uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
 					}
-					OccupancyGrid grid(parameters);
-					grid.setCellSize(cellSize);
-					for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
-					{
-						grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
-					}
+					OccupancyGrid grid(&combinedLocalMaps, parameters);
 					grid.update(graphFiltered);
 					if(ui_->checkBox_grid_showProbMap->isChecked())
 					{
@@ -7022,7 +6999,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}

 			// Generate 3d grid map?
-			if(ui_->dockWidget_occupancyGridView->isVisible())
+			if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_grid->isChecked())
 			{
 #ifdef RTABMAP_OCTOMAP
 				if(ui_->checkBox_octomap->isChecked())
@@ -7032,116 +7009,66 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				else
 #endif
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCellsXYZ(new pcl::PointCloud<pcl::PointXYZ>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
+					CloudMap cloudMap(&combinedLocalMaps, parameters);
+
+					cloudMap.update(graphFiltered);
+
+					const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCells = cloudMap.getMapGround();
+					const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacleCells = cloudMap.getMapObstacles();
+					const pcl::PointCloud<pcl::PointXYZ>::Ptr & emptyCells = cloudMap.getMapEmptyCells();

-					for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
-					{
-						Transform pose = graphFiltered.at(iter->first);
-						float x,y,z,roll,pitch,yaw;
-						pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
-						Transform pose2d(x,y, 0, 0, 0, yaw);
-						if(!iter->second.first.first.empty())
-						{
-							if(iter->second.first.first.channels() == 4)
-							{
-								*groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose);
-							}
-							else
-							{
-								*groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose);
-							}
-						}
-						if(!iter->second.first.second.empty())
-						{
-							if(iter->second.first.second.channels() == 4)
-							{
-								*obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose);
-							}
-							else
-							{
-								*obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose);
-							}
-						}
-						if(ui_->checkBox_grid_empty->isChecked())
-						{
-							if(!iter->second.second.empty())
-							{
-								if(iter->second.second.channels() == 4)
-								{
-									*emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose);
-								}
-								else
-								{
-									*emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose);
-								}
-							}
-						}
-					}
 					// occupancy cloud
-					if(groundRGB->size())
+					if(groundCells->size())
 					{
-						groundRGB = util3d::voxelize(groundRGB, cellSize);
-						occupancyGridViewer_->addCloud("groundRGB",
-								groundRGB,
+						occupancyGridViewer_->addCloud("groundCells",
+								groundCells,
 								Transform::getIdentity(),
 								QColor(ui_->lineEdit_groundColor->text()));
-						occupancyGridViewer_->setCloudPointSize("groundRGB", 5);
+						occupancyGridViewer_->setCloudPointSize("groundCells", 5);
 					}
-					if(groundXYZ->size())
+					if(obstacleCells->size())
 					{
-						groundXYZ = util3d::voxelize(groundXYZ, cellSize);
-						occupancyGridViewer_->addCloud("groundXYZ",
-								groundXYZ,
-								Transform::getIdentity(),
-								QColor(ui_->lineEdit_groundColor->text()));
-						occupancyGridViewer_->setCloudPointSize("groundXYZ", 5);
-					}
-					if(obstaclesRGB->size())
-					{
-						obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
-						occupancyGridViewer_->addCloud("obstaclesRGB",
-								obstaclesRGB,
+						occupancyGridViewer_->addCloud("obstacleCells",
+								obstacleCells,
 								Transform::getIdentity(),
 								QColor(ui_->lineEdit_obstacleColor->text()));
-						occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5);
+						occupancyGridViewer_->setCloudPointSize("obstacleCells", 5);
 					}
-					if(obstaclesXYZ->size())
+					if(ui_->checkBox_grid_empty->isChecked() && emptyCells->size())
 					{
-						obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
-						occupancyGridViewer_->addCloud("obstaclesXYZ",
-								obstaclesXYZ,
-								Transform::getIdentity(),
-								QColor(ui_->lineEdit_obstacleColor->text()));
-						occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
-					}
-					if(emptyCellsRGB->size())
-					{
-						emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
-						occupancyGridViewer_->addCloud("emptyCellsRGB",
-								emptyCellsRGB,
+						occupancyGridViewer_->addCloud("emptyCells",
+								emptyCells,
 								Transform::getIdentity(),
 								QColor(ui_->lineEdit_emptyColor->text()));
-						occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 5);
-						occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
-					}
-					if(emptyCellsXYZ->size())
-					{
-						emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
-						occupancyGridViewer_->addCloud("emptyCellsXYZ",
-								emptyCellsXYZ,
-								Transform::getIdentity(),
-								QColor(ui_->lineEdit_emptyColor->text()));
-						occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 5);
-						occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
+						occupancyGridViewer_->setCloudPointSize("emptyCells", 5);
+						occupancyGridViewer_->setCloudOpacity("emptyCells", 0.5);
 					}
 					occupancyGridViewer_->refreshView();
 				}
 			}
+
+#ifdef RTABMAP_GRIDMAP
+			// Show elevation map ?
+			if(ui_->dockWidget_occupancyGridView->isVisible() &&
+			   ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked)
+			{
+				GridMap gridMap(&combinedLocalMaps, parameters);
+
+				gridMap.update(graphFiltered);
+				if(ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked)
+				{
+					float xMin, yMin;
+					cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, cellSize);
+					occupancyGridViewer_->addElevationMap(elevationMap, cellSize, xMin, yMin, 1.0f);
+				}
+				else
+				{
+					pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
+					occupancyGridViewer_->addCloudMesh("elevation_mesh", mesh);
+				}
+				occupancyGridViewer_->refreshView();
+			}
+#endif
 		}
 		ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
 		if(rectScaleItem != 0)
@@ -7704,9 +7631,11 @@ void DatabaseViewer::updateGrid()
 		ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+		ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+		ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);

 		update3dView();
 		updateGraphView();
@@ -7719,9 +7648,11 @@ void DatabaseViewer::updateOctomapView()
 		ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+		ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
+		ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);

 		if(ui_->checkBox_octomap->isChecked())
 		{
@@ -7729,7 +7660,12 @@ void DatabaseViewer::updateOctomapView()
 			{
 				occupancyGridViewer_->removeOctomap();
 				occupancyGridViewer_->removeCloud("octomap_obstacles");
+				occupancyGridViewer_->removeCloud("octomap_ground");
 				occupancyGridViewer_->removeCloud("octomap_empty");
+				occupancyGridViewer_->removeCloud("octomap_frontiers");
+				occupancyGridViewer_->removeCloud("groundCells");
+				occupancyGridViewer_->removeCloud("obstacleCells");
+				occupancyGridViewer_->removeCloud("emptyCells");
 				if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
 				{
 					occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
@@ -7739,6 +7675,7 @@ void DatabaseViewer::updateOctomapView()
 					pcl::IndicesPtr obstacles(new std::vector<int>);
 					pcl::IndicesPtr empty(new std::vector<int>);
 					pcl::IndicesPtr ground(new std::vector<int>);
+					pcl::IndicesPtr frontiers(new std::vector<int>);
 					std::vector<double> prob;
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
 							ui_->spinBox_grid_depth->value(),
@@ -7746,7 +7683,7 @@ void DatabaseViewer::updateOctomapView()
 							empty.get(),
 							ground.get(),
 							true,
-							0,
+							frontiers.get(),
 							&prob);

 					if(octomap_->hasColor())
@@ -7806,6 +7743,15 @@ void DatabaseViewer::updateOctomapView()
 							occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
 						}
 					}
+
+					if(ui_->checkBox_grid_frontiers->isChecked())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr frontiersCloud(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud);
+						occupancyGridViewer_->addCloud("octomap_frontiers", frontiersCloud, Transform::getIdentity(), QColor(ui_->lineEdit_frontierColor->text()));
+						occupancyGridViewer_->setCloudOpacity("octomap_frontiers", 0.5);
+						occupancyGridViewer_->setCloudPointSize("octomap_frontiers", 5);
+					}
 				}
 				occupancyGridViewer_->refreshView();
 			}
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/RegistrationVis.h"
-#include "rtabmap/core/OccupancyGrid.h"
+#include "rtabmap/core/global_map/OccupancyGrid.h"
 #include "rtabmap/core/GainCompensator.h"
 #include "rtabmap/core/Recovery.h"
 #include "rtabmap/core/util2d.h"
@@ -115,7 +115,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/search/kdtree.h>

 #ifdef RTABMAP_OCTOMAP
-#include <rtabmap/core/OctoMap.h>
+#include <rtabmap/core/global_map/OctoMap.h>
+#endif
+
+#ifdef RTABMAP_GRIDMAP
+#include <rtabmap/core/global_map/GridMap.h>
 #endif

 #ifdef HAVE_OPENCV_ARUCO
@@ -164,6 +168,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_createdCloudsMemoryUsage(0),
 	_occupancyGrid(0),
 	_octomap(0),
+	_elevationMap(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
 	_oneSecondTimer(0),
@@ -259,9 +264,12 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());

 	ParametersMap parameters = _preferencesDialog->getAllParameters();
-	_occupancyGrid = new OccupancyGrid(parameters);
+	_occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters);
 #ifdef RTABMAP_OCTOMAP
-	_octomap = new OctoMap(parameters);
+	_octomap = new OctoMap(&_cachedLocalMaps, parameters);
+#endif
+#ifdef RTABMAP_GRIDMAP
+	_elevationMap = new GridMap(&_cachedLocalMaps, parameters);
 #endif

 	// Timer
@@ -666,6 +674,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 #ifdef RTABMAP_OCTOMAP
 	_ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
 	_ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
+#endif
+#ifdef RTABMAP_GRIDMAP
+	_ui->statsToolBox->updateStat("GUI/Elevation Update/ms", false);
+	_ui->statsToolBox->updateStat("GUI/Elevation Rendering/ms", false);
 #endif
 	_ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
 	_ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
@@ -703,6 +715,9 @@ MainWindow::~MainWindow()
 	delete _logEventTime;
 #ifdef RTABMAP_OCTOMAP
 	delete _octomap;
+#endif
+#ifdef RTABMAP_GRIDMAP
+	delete _elevationMap;
 #endif
 	delete _occupancyGrid;
 	UDEBUG("");
@@ -2960,13 +2975,20 @@ void MainWindow::updateMapCloud(
 					 (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
 					_occupancyGrid->addedNodes().find(iter->first) == _occupancyGrid->addedNodes().end();
 			bool updateOctomap = false;
+			bool updateElevationMap = false;
 #ifdef RTABMAP_OCTOMAP
 			updateOctomap =
 					_cloudViewer->isVisible() &&
 					_preferencesDialog->isOctomapUpdated() &&
 					_octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
 #endif
-			if(updateGridMap || updateOctomap)
+#ifdef RTABMAP_GRIDMAP
+			updateElevationMap =
+					_cloudViewer->isVisible() &&
+					_preferencesDialog->getElevationMapShown() > 0 &&
+					_elevationMap->addedNodes().find(iter->first) == _elevationMap->addedNodes().end();
+#endif
+			if(updateGridMap || updateOctomap || updateElevationMap)
 			{
 				QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
 				if(jter!=_cachedSignatures.end() && jter->sensorData().gridCellSize() > 0.0f)
@@ -2977,23 +2999,7 @@ void MainWindow::updateMapCloud(

 					jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);

-					_occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
-
-#ifdef RTABMAP_OCTOMAP
-					if(updateOctomap)
-					{
-						if((ground.empty() || ground.channels() > 2) &&
-						   (obstacles.empty() || obstacles.channels() > 2))
-						{
-							cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
-							_octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
-						}
-						else if(!ground.empty() || !obstacles.empty())
-						{
-							UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
-						}
-					}
-#endif
+					_cachedLocalMaps.add(iter->first, ground, obstacles, empty, jter->sensorData().gridCellSize(), jter->sensorData().gridViewPoint());
 				}
 			}

@@ -3332,6 +3338,50 @@ void MainWindow::updateMapCloud(
 	}
 #endif

+#ifdef RTABMAP_GRIDMAP
+	_cloudViewer->removeElevationMap();
+	_cloudViewer->removeCloud("elevation_mesh");
+	if(_preferencesDialog->getElevationMapShown() > 0)
+	{
+		UDEBUG("");
+		UTimer time;
+		_elevationMap->update(poses);
+		UINFO("Elevation map update time = %fs", time.ticks());
+	}
+	if(stats)
+	{
+		stats->insert(std::make_pair("GUI/Elevation Update/ms", (float)timer.restart()*1000.0f));
+	}
+	if(_preferencesDialog->getElevationMapShown() > 0)
+	{
+		UDEBUG("");
+		UTimer time;
+		if(_preferencesDialog->getElevationMapShown() == 1)
+		{
+			float xMin, yMin, cellSize;
+			cv::Mat map = _elevationMap->createHeightMap(xMin, yMin, cellSize);
+			if(!map.empty())
+			{
+				_cloudViewer->addElevationMap(map, cellSize, xMin, yMin, 1);
+			}
+		}
+		else // RGB elevation
+		{
+			pcl::PolygonMesh::Ptr mesh = _elevationMap->createTerrainMesh();
+			if(mesh->cloud.data.size())
+			{
+				_cloudViewer->addCloudMesh("elevation_mesh", mesh);
+			}
+		}
+		UINFO("Show elevation map time = %fs", time.ticks());
+	}
+	UDEBUG("");
+	if(stats)
+	{
+		stats->insert(std::make_pair("GUI/Elevation Rendering/ms", (float)timer.restart()*1000.0f));
+	}
+#endif
+
 	// Add landmarks to 3D Map view
 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
 	_cloudViewer->removeAllCoordinates("landmark_");
@@ -3392,7 +3442,7 @@ void MainWindow::updateMapCloud(
 		else
 #endif
 		{
-			if(_occupancyGrid->addedNodes().size() || _occupancyGrid->cacheSize()>0)
+			if(_occupancyGrid->addedNodes().size() || _cachedLocalMaps.size()>0)
 			{
 				_occupancyGrid->update(poses);
 			}
@@ -3420,6 +3470,8 @@ void MainWindow::updateMapCloud(
 	}
 	_ui->graphicsView_graphView->update();

+	_cachedLocalMaps.clear(true);
+
 	UDEBUG("");
 	if(stats)
 	{
@@ -4845,7 +4897,6 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
 {
 	ULOGGER_DEBUG("");
-	_occupancyGrid->parseParameters(_preferencesDialog->getAllParameters());
 	if(parameters.size())
 	{
 		for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
@@ -5922,13 +5973,19 @@ void MainWindow::startDetection()
 				   "progress will not be shown in the GUI."));
 	}

-	_occupancyGrid->clear();
-	_occupancyGrid->parseParameters(parameters);
+	_cachedLocalMaps.clear();
+
+	delete _occupancyGrid;
+	_occupancyGrid = new OccupancyGrid(&_cachedLocalMaps, parameters);

 #ifdef RTABMAP_OCTOMAP
-	UASSERT(_octomap != 0);
 	delete _octomap;
-	_octomap = new OctoMap(parameters);
+	_octomap = new OctoMap(&_cachedLocalMaps, parameters);
+#endif
+
+#ifdef RTABMAP_GRIDMAP
+	delete _elevationMap;
+	_elevationMap = new GridMap(&_cachedLocalMaps, parameters);
 #endif

 	// clear odometry visual stuff
@@ -7403,11 +7460,11 @@ void MainWindow::clearTheCache()
 	_ui->imageView_loopClosure->setBackgroundColor(_ui->imageView_loopClosure->getDefaultBackgroundColor());
 	_ui->imageView_odometry->setBackgroundColor(_ui->imageView_odometry->getDefaultBackgroundColor());
 	_multiSessionLocWidget->clear();
+	_cachedLocalMaps.clear();
 #ifdef RTABMAP_OCTOMAP
 	// re-create one if the resolution has changed
-	UASSERT(_octomap != 0);
 	delete _octomap;
-	_octomap = new OctoMap(_preferencesDialog->getAllParameters());
+	_octomap = new OctoMap(&_cachedLocalMaps, _preferencesDialog->getAllParameters());
 #endif
 	_occupancyGrid->clear();
 	_rectCameraModels.clear();
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -158,6 +158,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->groupBox_octomap->setEnabled(false);
 #endif

+#ifndef RTABMAP_GRIDMAP
+	_ui->checkBox_elevation_shown->setChecked(false);
+	_ui->checkBox_elevation_shown->setEnabled(false);
+	_ui->label_show_elevation->setEnabled(false);
+#endif
+
 #ifndef RTABMAP_REALSENSE_SLAM
 	_ui->checkbox_realsenseOdom->setChecked(false);
 	_ui->checkbox_realsenseOdom->setEnabled(false);
@@ -621,6 +627,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_elevation_shown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));

 	connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1269,7 +1276,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_grid_unknownSpaceFilled->setObjectName(Parameters::kGridScan2dUnknownSpaceFilled().c_str());
 	_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());

-	_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str());
 	_ui->doubleSpinBox_grid_updateError->setObjectName(Parameters::kGridGlobalUpdateError().c_str());
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
 	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
@@ -1981,6 +1987,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 	{
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
+		_ui->checkBox_elevation_shown->setCheckState(Qt::Unchecked);

 		_ui->groupBox_octomap->setChecked(false);
 		_ui->spinBox_octomap_treeDepth->setValue(16);
@@ -2462,6 +2469,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)

 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
+	_ui->checkBox_elevation_shown->setCheckState((Qt::CheckState)settings.value("elevationMapShown", _ui->checkBox_elevation_shown->checkState()).toInt());

 	_ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool());
 	_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
@@ -3001,6 +3009,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const

 	settings.setValue("gridMapShown",                _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapOpacity",              _ui->doubleSpinBox_map_opacity->value());
+	settings.setValue("elevationMapShown",           _ui->checkBox_elevation_shown->checkState());
+

 	settings.setValue("octomap",                     _ui->groupBox_octomap->isChecked());
 	settings.setValue("octomap_depth",               _ui->spinBox_octomap_treeDepth->value());
@@ -5841,6 +5851,10 @@ bool PreferencesDialog::getGridMapShown() const
 {
 	return _ui->checkBox_map_shown->isChecked();
 }
+int PreferencesDialog::getElevationMapShown() const
+{
+	return _ui->checkBox_elevation_shown->checkState();
+}
 int PreferencesDialog::getGridMapSensor() const
 {
 	return _ui->comboBox_grid_sensor->currentIndex();
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -1722,7 +1722,7 @@
          <x>0</x>
          <y>0</y>
          <width>518</width>
-          <height>951</height>
+          <height>1007</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1731,7 +1731,44 @@
        <layout class="QVBoxLayout" name="verticalLayout_16">
         <item>
          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
-           <item row="11" column="0">
+           <item row="6" column="1">
+            <widget class="QLabel" name="label_53">
+             <property name="text">
+              <string>OctoMap</string>
+             </property>
+            </widget>
+           </item>
+           <item row="15" column="1">
+            <widget class="QLabel" name="label_57">
+             <property name="text">
+              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="15" column="0">
+            <widget class="QSpinBox" name="spinBox_cropRadius">
+             <property name="suffix">
+              <string> pixels</string>
+             </property>
+             <property name="value">
+              <number>1</number>
+             </property>
+            </widget>
+           </item>
+           <item row="16" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="13" column="0">
            <widget class="QSpinBox" name="spinBox_decimation">
             <property name="prefix">
              <string/>
@@ -1747,86 +1784,23 @@
             </property>
            </widget>
           </item>
-           <item row="2" column="1">
-            <widget class="QLabel" name="label_octomap_empty_3">
-             <property name="text">
-              <string>Ground cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="13" column="1">
-            <widget class="QLabel" name="label_57">
-             <property name="text">
-              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="9" column="1">
-            <widget class="QLabel" name="label_55">
-             <property name="text">
-              <string>Local grid: regenerate from saved grid instead of sensors</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="12" column="0">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
-             <property name="suffix">
-              <string> m</string>
-             </property>
-             <property name="decimals">
-              <number>3</number>
+           <item row="10" column="0">
+            <widget class="QSpinBox" name="spinBox_grid_depth">
+             <property name="prefix">
+              <string/>
             </property>
             <property name="minimum">
-              <double>0.000000000000000</double>
+              <number>0</number>
             </property>
             <property name="maximum">
-              <double>99.000000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
+              <number>16</number>
             </property>
             <property name="value">
-              <double>0.000000000000000</double>
+              <number>16</number>
             </property>
            </widget>
           </item>
-           <item row="3" column="1">
-            <widget class="QLabel" name="label_octomap_empty_4">
-             <property name="text">
-              <string>Empty cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="9" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
-             <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="0">
-            <widget class="QCheckBox" name="checkBox_octomap">
-             <property name="text">
-              <string/>
-             </property>
-            </widget>
-           </item>
-           <item row="2" column="0">
+           <item row="3" column="0">
            <layout class="QHBoxLayout" name="horizontalLayout_9">
             <item>
              <widget class="QToolButton" name="toolButton_groundColor">
@@ -1844,66 +1818,27 @@
             </item>
            </layout>
           </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_octomap_empty">
+           <item row="9" column="1">
+            <widget class="QLabel" name="label_octomap_cubes">
             <property name="text">
-              <string>Show empty space</string>
+              <string>OctoMap: Rendering type</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-           <item row="10" column="1">
-            <widget class="QLabel" name="label_51">
+           <item row="1" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_frontiers">
             <property name="text">
-              <string>Gain compensation radius (Constraints view)</string>
+              <string/>
             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
+             <property name="checked">
+              <bool>false</bool>
             </property>
            </widget>
           </item>
           <item row="14" column="1">
-            <widget class="QLabel" name="label_59">
-             <property name="text">
-              <string>Show probabilistic occupancy grid in Graph View.</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="7" column="0">
-            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
-             <item>
-              <property name="text">
-               <string>Point Cloud</string>
-              </property>
-             </item>
-             <item>
-              <property name="text">
-               <string>Cube</string>
-              </property>
-             </item>
-             <item>
-              <property name="text">
-               <string>Volume</string>
-              </property>
-             </item>
-            </widget>
-           </item>
-           <item row="11" column="1">
-            <widget class="QLabel" name="label_56">
-             <property name="text">
-              <string>Decimation (for images)</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="12" column="1">
            <widget class="QLabel" name="label_54">
             <property name="text">
              <string>Voxel size (for clouds and scans)</string>
@@ -1913,43 +1848,94 @@
             </property>
            </widget>
           </item>
-           <item row="13" column="0">
-            <widget class="QSpinBox" name="spinBox_cropRadius">
-             <property name="suffix">
-              <string> pixels</string>
-             </property>
-             <property name="value">
-              <number>1</number>
-             </property>
-            </widget>
-           </item>
-           <item row="8" column="0">
-            <widget class="QSpinBox" name="spinBox_grid_depth">
-             <property name="prefix">
-              <string/>
-             </property>
-             <property name="minimum">
-              <number>0</number>
-             </property>
-             <property name="maximum">
-              <number>16</number>
-             </property>
-             <property name="value">
-              <number>16</number>
-             </property>
-            </widget>
-           </item>
-           <item row="15" column="1">
-            <widget class="QLabel" name="label_60">
+           <item row="10" column="1">
+            <widget class="QLabel" name="label_octomap_depth">
             <property name="text">
-              <string>Create RGB-D cloud from RGB projection on scan.</string>
+              <string>OctoMap: Tree depth</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-           <item row="10" column="0">
+           <item row="11" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="6" column="0">
+            <widget class="QCheckBox" name="checkBox_octomap">
+             <property name="text">
+              <string/>
+             </property>
+            </widget>
+           </item>
+           <item row="2" column="1">
+            <widget class="QLabel" name="label_octomap_empty_2">
+             <property name="text">
+              <string>Obstacle cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="12" column="1">
+            <widget class="QLabel" name="label_51">
+             <property name="text">
+              <string>Gain compensation radius (Constraints view)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="18" column="0">
+            <widget class="QCheckBox" name="checkBox_showDisparityInsteadOfRight">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="16" column="1">
+            <widget class="QLabel" name="label_59">
+             <property name="text">
+              <string>Show probabilistic occupancy grid in Graph View.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="18" column="1">
+            <widget class="QLabel" name="label_logger_level_2">
+             <property name="text">
+              <string>For stereo data, show disparity instead of right image in main views.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="3" column="1">
+            <widget class="QLabel" name="label_octomap_empty_3">
+             <property name="text">
+              <string>Ground cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="12" column="0">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
             <property name="suffix">
              <string> m</string>
@@ -1971,7 +1957,17 @@
             </property>
            </widget>
           </item>
-           <item row="3" column="0">
+           <item row="4" column="1">
+            <widget class="QLabel" name="label_octomap_empty_4">
+             <property name="text">
+              <string>Empty cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="0">
            <layout class="QHBoxLayout" name="horizontalLayout_12">
             <item>
              <widget class="QToolButton" name="toolButton_emptyColor">
@@ -1989,17 +1985,7 @@
             </item>
            </layout>
           </item>
-           <item row="14" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
-             <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="1" column="0">
+           <item row="2" column="0">
            <layout class="QHBoxLayout" name="horizontalLayout_11">
             <item>
              <widget class="QToolButton" name="toolButton_obstacleColor">
@@ -2017,17 +2003,7 @@
             </item>
            </layout>
           </item>
-           <item row="1" column="1">
-            <widget class="QLabel" name="label_octomap_empty_2">
-             <property name="text">
-              <string>Obstacle cell color</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="15" column="0">
+           <item row="17" column="0">
            <widget class="QCheckBox" name="checkBox_cameraProjection">
             <property name="text">
              <string/>
@@ -2037,33 +2013,6 @@
             </property>
            </widget>
           </item>
-           <item row="8" column="1">
-            <widget class="QLabel" name="label_octomap_depth">
-             <property name="text">
-              <string>OctoMap: Tree depth</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="1">
-            <widget class="QLabel" name="label_53">
-             <property name="text">
-              <string>OctoMap</string>
-             </property>
-            </widget>
-           </item>
-           <item row="7" column="1">
-            <widget class="QLabel" name="label_octomap_cubes">
-             <property name="text">
-              <string>OctoMap: Rendering type</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
           <item row="0" column="0">
            <widget class="QCheckBox" name="checkBox_grid_empty">
             <property name="text">
@@ -2074,26 +2023,125 @@
             </property>
            </widget>
           </item>
-           <item row="16" column="1">
-            <widget class="QLabel" name="label_logger_level_2">
+           <item row="11" column="1">
+            <widget class="QLabel" name="label_55">
             <property name="text">
-              <string>For stereo data, show disparity instead of right image in main views.</string>
+              <string>Local grid: regenerate from saved grid instead of sensors</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
             </property>
            </widget>
           </item>
-           <item row="16" column="0">
-            <widget class="QCheckBox" name="checkBox_showDisparityInsteadOfRight">
+           <item row="1" column="1">
+            <widget class="QLabel" name="label_octomap_frontiers">
             <property name="text">
-              <string/>
+              <string>Show frontiers</string>
             </property>
-             <property name="checked">
-              <bool>false</bool>
+             <property name="wordWrap">
+              <bool>true</bool>
             </property>
            </widget>
           </item>
+           <item row="17" column="1">
+            <widget class="QLabel" name="label_60">
+             <property name="text">
+              <string>Create RGB-D cloud from RGB projection on scan.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="1">
+            <widget class="QLabel" name="label_octomap_empty">
+             <property name="text">
+              <string>Show empty space</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="9" column="0">
+            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
+             <item>
+              <property name="text">
+               <string>Point Cloud</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Cube</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Volume</string>
+              </property>
+             </item>
+            </widget>
+           </item>
+           <item row="13" column="1">
+            <widget class="QLabel" name="label_56">
+             <property name="text">
+              <string>Decimation (for images)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="14" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
+             <property name="suffix">
+              <string> m</string>
+             </property>
+             <property name="decimals">
+              <number>3</number>
+             </property>
+             <property name="minimum">
+              <double>0.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>99.000000000000000</double>
+             </property>
+             <property name="singleStep">
+              <double>0.010000000000000</double>
+             </property>
+             <property name="value">
+              <double>0.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="5" column="1">
+            <widget class="QLabel" name="label_octomap_frontiers_3">
+             <property name="text">
+              <string>Frontier cell color</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="5" column="0">
+            <layout class="QHBoxLayout" name="horizontalLayout_17">
+             <item>
+              <widget class="QToolButton" name="toolButton_frontierColor">
+               <property name="text">
+                <string>...</string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLineEdit" name="lineEdit_frontierColor">
+               <property name="readOnly">
+                <bool>true</bool>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
          </layout>
         </item>
         <item>
@@ -2796,9 +2844,28 @@
        </widget>
       </item>
       <item>
-        <widget class="QCheckBox" name="checkBox_odomFrame_3dview">
+        <widget class="QCheckBox" name="checkBox_showElevation">
+         <property name="toolTip">
+          <string>Available if RTAB-Map is built with GridMap</string>
+         </property>
         <property name="text">
-          <string>Odom Frame</string>
+          <string>Elevation</string>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
+         <property name="tristate">
+          <bool>true</bool>
+         </property>
+        </widget>
+       </item>
+       <item>
+        <widget class="QCheckBox" name="checkBox_odomFrame_3dview">
+         <property name="toolTip">
+          <string>Adjust roll, pitch and height based on odometry pose</string>
+         </property>
+         <property name="text">
+          <string>Odom</string>
         </property>
         <property name="checked">
          <bool>true</bool>
@@ -2966,6 +3033,16 @@
     </item>
     <item>
      <layout class="QHBoxLayout" name="horizontalLayout_8">
+       <item>
+        <widget class="QCheckBox" name="checkBox_grid_grid">
+         <property name="text">
+          <string>Grid</string>
+         </property>
+         <property name="checked">
+          <bool>true</bool>
+         </property>
+        </widget>
+       </item>
       <item>
        <widget class="QCheckBox" name="checkBox_grid_2d">
         <property name="text">
@@ -2976,6 +3053,22 @@
         </property>
        </widget>
       </item>
+       <item>
+        <widget class="QCheckBox" name="checkBox_grid_elevation">
+         <property name="toolTip">
+          <string>Available if RTAB-Map is built with GridMap</string>
+         </property>
+         <property name="text">
+          <string>Elevation</string>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
+         <property name="tristate">
+          <bool>true</bool>
+         </property>
+        </widget>
+       </item>
       <item>
        <spacer name="horizontalSpacer_5">
         <property name="orientation">
--- a/guilib/src/ui/aboutDialog.ui
+++ b/guilib/src/ui/aboutDialog.ui
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -95,7 +95,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>5</number>
+                <number>3</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -2414,13 +2414,22 @@ Show a yellow background when the number of odometry inliers goes under this thr
                    </item>
                    <item>
                     <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
-                      <item row="0" column="0">
-                       <widget class="QCheckBox" name="checkBox_map_shown">
-                        <property name="text">
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
+                        <property name="suffix">
                         <string/>
                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
+                        <property name="minimum">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.050000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.750000000000000</double>
                        </property>
                       </widget>
                      </item>
@@ -2444,22 +2453,39 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        </property>
                       </widget>
                      </item>
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
-                        <property name="suffix">
+                      <item row="0" column="0">
+                       <widget class="QCheckBox" name="checkBox_map_shown">
+                        <property name="text">
                         <string/>
                        </property>
-                        <property name="minimum">
-                         <double>0.010000000000000</double>
+                        <property name="checked">
+                         <bool>false</bool>
                        </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_show_elevation">
+                        <property name="text">
+                         <string>Show Elevation Map in 3D map view.</string>
                        </property>
-                        <property name="singleStep">
-                         <double>0.050000000000000</double>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                        </property>
-                        <property name="value">
-                         <double>0.750000000000000</double>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QCheckBox" name="checkBox_elevation_shown">
+                        <property name="toolTip">
+                         <string>Partially checked: Height color map. Checked: RGB color map.</string>
+                        </property>
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                        <property name="tristate">
+                         <bool>true</bool>
                        </property>
                       </widget>
                      </item>
@@ -13297,7 +13323,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <layout class="QVBoxLayout" name="verticalLayout_115">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_88" columnstretch="0,1">
-                      <item row="5" column="0">
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_616">
+                        <property name="text">
+                         <string>Altitude delta. Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building).</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_grid_footprintRadius">
                        <property name="suffix">
                         <string> m</string>
@@ -13316,200 +13355,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="0">
-                       <widget class="QCheckBox" name="checkBox_grid_erode">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="8" column="1">
-                       <widget class="QLabel" name="label_483">
-                        <property name="text">
-                         <string>Probability of a miss.</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_grid_updateError">
-                        <property name="suffix">
-                         <string> m</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.001000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>99.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.010000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_366">
-                        <property name="text">
-                         <string>Minimum map size.</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_616">
-                        <property name="text">
-                         <string>Altitude delta. Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="0">
-                       <widget class="QCheckBox" name="checkBox_grid_fullUpdate">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="0">
-                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_occThr">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.000000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="9" column="0">
-                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMin">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="decimals">
-                         <number>4</number>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.500000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_332">
-                        <property name="text">
-                         <string>Full update. When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.</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="QDoubleSpinBox" name="doubleSpinBox_grid_probMiss">
-                        <property name="suffix">
-                         <string/>
-                        </property>
-                        <property name="minimum">
-                         <double>0.000000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>0.500000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.500000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_454">
-                        <property name="text">
-                         <string>Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value.</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="1">
-                       <widget class="QLabel" name="label_482">
-                        <property name="text">
-                         <string>Probability of a hit.</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="1">
-                       <widget class="QLabel" name="label_455">
-                        <property name="text">
-                         <string>Maximum nodes assembled in the map starting from the last node (0=unlimited).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="6" column="1">
+                      <item row="5" column="1">
                       <widget class="QLabel" name="label_457">
                        <property name="text">
                         <string>Occupancy threshold.</string>
@@ -13522,7 +13368,27 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_319">
+                        <property name="text">
+                         <string>Footprint radius used to clear all obstacles under the graph.</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_grid_maxNodes">
+                        <property name="maximum">
+                         <number>9999</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minMapSize">
                        <property name="suffix">
                         <string> m</string>
@@ -13544,7 +13410,187 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="0">
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_483">
+                        <property name="text">
+                         <string>Probability of a miss.</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="1">
+                       <widget class="QLabel" name="label_484">
+                        <property name="text">
+                         <string>Probability clamping threshold minimum.</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="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMin">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="decimals">
+                         <number>4</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_224">
+                        <property name="text">
+                         <string>Erode obstacle cells.</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="1">
+                       <widget class="QLabel" name="label_366">
+                        <property name="text">
+                         <string>Minimum map size.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_updateError">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>99.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probHit">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_occThr">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="0">
+                       <widget class="QSpinBox" name="spinBox_grid_floodfilldepth">
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_455">
+                        <property name="text">
+                         <string>Maximum nodes assembled in the map starting from the last node (0=unlimited).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_485">
+                        <property name="text">
+                         <string>Probability clamping threshold maximum.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="11" column="1">
+                       <widget class="QLabel" name="label_316">
+                        <property name="text">
+                         <string>Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMax">
                        <property name="suffix">
                         <string/>
@@ -13566,7 +13612,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="4" column="0">
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probMiss">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="0">
                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_altitudeDelta">
                        <property name="suffix">
                         <string> m</string>
@@ -13588,42 +13653,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="11" column="1">
-                       <widget class="QLabel" name="label_224">
+                      <item row="10" column="0">
+                       <widget class="QCheckBox" name="checkBox_grid_erode">
                        <property name="text">
-                         <string>Erode obstacle cells.</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_grid_probHit">
-                        <property name="suffix">
                         <string/>
                        </property>
-                        <property name="minimum">
-                         <double>0.500000000000000</double>
-                        </property>
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.500000000000000</double>
+                        <property name="checked">
+                         <bool>false</bool>
                        </property>
                       </widget>
                      </item>
-                      <item row="9" column="1">
-                       <widget class="QLabel" name="label_484">
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_482">
                        <property name="text">
-                         <string>Probability clamping threshold minimum.</string>
+                         <string>Probability of a hit.</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
@@ -13633,10 +13676,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_319">
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_454">
                        <property name="text">
-                         <string>Footprint radius used to clear all obstacles under the graph.</string>
+                         <string>Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value.</string>
                        </property>
                        <property name="wordWrap">
                         <bool>true</bool>
@@ -13646,46 +13689,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="10" column="1">
-                       <widget class="QLabel" name="label_485">
-                        <property name="text">
-                         <string>Probability clamping threshold maximum.</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="QSpinBox" name="spinBox_grid_maxNodes">
-                        <property name="maximum">
-                         <number>9999</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="12" column="1">
-                       <widget class="QLabel" name="label_316">
-                        <property name="text">
-                         <string>Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="12" column="0">
-                       <widget class="QSpinBox" name="spinBox_grid_floodfilldepth">
-                        <property name="maximum">
-                         <number>16</number>
-                        </property>
-                       </widget>
-                      </item>
                     </layout>
                    </item>
                   </layout>