0.11.10: Database update with occupancy grid and laser scan info. Added class LaserScanInfo and OccupancyGrid (incremental 2d grid map). Gui: 2d grid and octomap are udpated using occupancy grids saved in nodes.

2026-09-03 10:00:23 +08:00 · 2016-08-21 19:33:01 -04:00
parent af02e02978
commit 013eba1d58
49 changed files with 3376 additions and 1259 deletions
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -294,7 +294,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	
 	connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -431,7 +431,7 @@ void DatabaseViewer::readSettings()
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
 	ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
 	ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
-	ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
+	ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
 	settings.endGroup();
 	// Visual parameters
 	settings.beginGroup("visual");
@@ -519,7 +519,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
 	settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
 	settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
-	settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
+	settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
 	settings.endGroup();
 	
 	// save Visual parameters
@@ -893,8 +893,9 @@ void DatabaseViewer::exportDatabase()
 					{
 						sensorData = rtabmap::SensorData(
 							scan,
-							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
-							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
+							LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
 							rgb,
 							depth,
 							data.cameraModels(),
@@ -906,8 +907,9 @@ void DatabaseViewer::exportDatabase()
 					{
 						sensorData = rtabmap::SensorData(
 							scan,
-							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
-							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
+							LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
 							rgb,
 							depth,
 							data.stereoCameraModel(),
@@ -2423,7 +2425,7 @@ void DatabaseViewer::update(int value,
 					}

 					//add scan
-					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
 					if(scan->size())
 					{
 						view3D->addCloud("1", scan);
@@ -3306,8 +3308,8 @@ void DatabaseViewer::updateConstraintView(

 				// Added loop closure scans
 				pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
-				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
+				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
+				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), dataTo.laserScanInfo().localTransform());
 				scanB = rtabmap::util3d::transformPointCloud(scanB, t);
 				if(scanA->size())
 				{
@@ -3509,7 +3511,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 										obstacles,
 										ui_->doubleSpinBox_gridCellSize->value(),
 										ui_->checkBox_gridFillUnkownSpace->isChecked(),
-										data.laserScanMaxRange());
+										data.laserScanInfo().maxRange());
 								added = true;
 							}
 							localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
@@ -3904,7 +3906,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	ParametersMap parameters = ui_->parameters_toolbox->getParameters();

 	UTimer timer;
-	if(ui_->checkBox_icp_laserScan->isChecked())
+	if(ui_->checkBox_icp_from_depth->isChecked())
 	{
 		// generate laser scans from depth image
 		cv::Mat tmpA, tmpB, tmpC, tmpD;
@@ -3925,8 +3927,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 				0,
 				ui_->parameters_toolbox->getParameters());
 		int maxLaserScans = cloudFrom->size();
-		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
-		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
+		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), LaserScanInfo(maxLaserScans, 0));
+		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), LaserScanInfo(maxLaserScans, 0));

 		if(!dataFrom.laserScanCompressed().empty() || !dataTo.laserScanCompressed().empty())
 		{
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -40,6 +40,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/gui/ImageView.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -151,6 +152,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_waypointsIndex(0),
 	_cachedMemoryUsage(0),
 	_createdCloudsMemoryUsage(0),
+	_cachedGridsMemoryUsage(0),
+	_occupancyGrid(0),
 	_octomap(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
@@ -234,6 +237,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_preferencesDialog->loadWindowGeometry(_aboutDialog);
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());

+	_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
 #ifdef RTABMAP_OCTOMAP
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif
@@ -568,6 +572,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", 0.0f);
 	_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", 0.0f);
 	_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", 0.0f);
+	_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", 0.0f);
+#ifdef RTABMAP_OCTOMAP
+	_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", 0.0f);
+#endif

 	this->loadFigures();
 	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
@@ -589,6 +597,10 @@ MainWindow::~MainWindow()
 	this->stopDetection();
 	delete _ui;
 	delete _elapsedTime;
+#ifdef RTABMAP_OCTOMAP
+	delete _octomap;
+#endif
+	delete _occupancyGrid;
 	UDEBUG("");
 }

@@ -1024,7 +1036,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					}

 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloudNormal(scan, pose);
+					cloud = util3d::laserScanToPointCloudNormal(scan, odom.data().laserScanInfo().localTransform()*pose);
 					if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
 					{
 						cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
@@ -1364,14 +1376,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)

 			if(!smallMovement)
 			{
-				// keep in cache only compressed data
-				Signature signatureWithoutRawData = signature;
-				signatureWithoutRawData.sensorData().setImageRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setDepthOrRightRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setUserDataRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setLaserScanRaw(cv::Mat(), 0, 0);
-				_cachedSignatures.insert(signature.id(), signatureWithoutRawData);
-				_cachedMemoryUsage += signatureWithoutRawData.sensorData().getMemoryUsed();
+				_cachedSignatures.insert(signature.id(), signature);
+				_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
 			}
 		}

@@ -1802,6 +1808,21 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
 			}
 		}
+		UDEBUG("");
+
+		// keep only compressed data in cache
+		if(_cachedSignatures.contains(stat.refImageId()))
+		{
+			Signature & s = *_cachedSignatures.find(stat.refImageId());
+			_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
+			s.sensorData().setImageRaw(cv::Mat());
+			s.sensorData().setDepthOrRightRaw(cv::Mat());
+			s.sensorData().setUserDataRaw(cv::Mat());
+			s.sensorData().setLaserScanRaw(cv::Mat(), signature.sensorData().laserScanInfo());
+			s.sensorData().clearOccupancyGridRaw();
+			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
+		}
+

 		UDEBUG("");
 	}
@@ -1829,7 +1850,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	}
 	_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024));
 	_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024));
-
+	_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedGridsMemoryUsage/(1024*1024));
+#ifdef RTABMAP_OCTOMAP
+	_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024));
+#endif
 	if(_state != kMonitoring && _state != kDetecting)
 	{
 		_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
@@ -1936,19 +1960,7 @@ void MainWindow::updateMapCloud(

 			// 3d point cloud
 			bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
-			bool updateProjMap =
-					_ui->graphicsView_graphView->isVisible() &&
-					_ui->graphicsView_graphView->isGridMapVisible() &&
-					_preferencesDialog->isGridMapFrom3DCloud() &&
-					_projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end();
-			bool updateOctomap = false;
-#ifdef RTABMAP_OCTOMAP
-			updateOctomap =
-					_cloudViewer->isVisible() &&
-					_preferencesDialog->isOctomapShown() &&
-					_octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
-#endif
-			if(update3dCloud ||	updateProjMap || updateOctomap)
+			if(update3dCloud)
 			{
 				// update cloud
 				std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
@@ -1976,20 +1988,6 @@ void MainWindow::updateMapCloud(
 						_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
 					}
 				}
-
-				//Update projection map
-				if(updateProjMap || updateOctomap)
-				{
-					std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first);
-					if(cloudIter != _cachedClouds.end())
-					{
-						createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second, updateOctomap);
-					}
-					else if(createdCloud.first.get() && createdCloud.first->size() && createdCloud.second->size())
-					{
-						createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second, updateOctomap);
-					}
-				}
 			}
 			else if(viewerClouds.contains(cloudName))
 			{
@@ -1998,8 +1996,7 @@ void MainWindow::updateMapCloud(

 			// 2d point cloud
 			std::string scanName = uFormat("scan%d", iter->first);
-			if((_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
-				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
+			if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
 			{
 				if(viewerClouds.contains(scanName))
 				{
@@ -2031,6 +2028,59 @@ void MainWindow::updateMapCloud(
 				_cloudViewer->setCloudVisibility(scanName.c_str(), false);
 			}

+			// occupancy grids
+			bool updateGridMap =
+					((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
+					 (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
+					_gridLocalMaps.find(iter->first) == _gridLocalMaps.end();
+			bool updateOctomap = false;
+#ifdef RTABMAP_OCTOMAP
+			updateOctomap =
+					_cloudViewer->isVisible() &&
+					_preferencesDialog->isOctomapUpdated() &&
+					_octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
+#endif
+			if(updateGridMap || updateOctomap)
+			{
+				QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+				if(jter!=_cachedSignatures.end())
+				{
+					if(_gridLocalMaps.find(iter->first) == _gridLocalMaps.end())
+					{
+						cv::Mat ground;
+						cv::Mat obstacles;
+						jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+						_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+						_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
+						_cachedGridsMemoryUsage += ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize();
+
+						if(ground.cols || obstacles.cols)
+						{
+							_occupancyGrid->addToCache(iter->first, ground, obstacles);
+						}
+					}
+#ifdef RTABMAP_OCTOMAP
+					if(updateOctomap)
+					{
+						std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = _gridLocalMaps.find(iter->first);
+						std::map<int, cv::Point3f>::iterator pter = _gridViewPoints.find(iter->first);
+						if(mter != _gridLocalMaps.end() && pter!=_gridViewPoints.end())
+						{
+							if((mter->second.first.empty() || mter->second.first.channels() > 2) &&
+							   (mter->second.second.empty() || mter->second.second.channels() > 2))
+							{
+								_octomap->addToCache(iter->first, mter->second.first, mter->second.second, pter->second);
+							}
+							else if(!mter->second.first.empty() && !mter->second.second.empty())
+							{
+								UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
+							}
+						}
+					}
+#endif
+				}
+			}
+
 			// 3d features
 			std::string featuresName = uFormat("features%d", iter->first);
 			if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
@@ -2084,7 +2134,7 @@ void MainWindow::updateMapCloud(
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -2196,19 +2246,37 @@ void MainWindow::updateMapCloud(
 	}
 	cv::Mat map8U;
 	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) &&
-			((_gridLocalMaps.size() && !_preferencesDialog->isGridMapFrom3DCloud()) ||
-			 (_projectionLocalMaps.size() && _preferencesDialog->isGridMapFrom3DCloud())))
+		_gridLocalMaps.size())
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();
-		cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
-					poses,
-					_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
-					resolution,
-					xMin, yMin,
-					0,
-					_preferencesDialog->isGridMapEroded(),
-					_preferencesDialog->getGridMapFootprintRadius());
+		cv::Mat map8S;
+#ifdef RTABMAP_OCTOMAP
+		if(_preferencesDialog->isOctomap2dGrid())
+		{
+			map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0);
+
+		}
+		else
+#endif
+		{
+			if(_preferencesDialog->isGridMapIncremental())
+			{
+				_occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius());
+				map8S = _occupancyGrid->getMap(xMin, yMin);
+			}
+			else
+			{
+				map8S = util3d::create2DMapFromOccupancyLocalMaps(
+							poses,
+							_gridLocalMaps,
+							resolution,
+							xMin, yMin,
+							0,
+							_preferencesDialog->isGridMapEroded(),
+							_preferencesDialog->getGridMapFootprintRadius());
+			}
+		}
 		if(!map8S.empty())
 		{
 			//convert to gray scaled map
@@ -2237,14 +2305,20 @@ void MainWindow::updateMapCloud(

 #ifdef RTABMAP_OCTOMAP
 	_cloudViewer->removeOctomap();
-	if(_preferencesDialog->isOctomapShown())
+	if(_preferencesDialog->isOctomapUpdated())
 	{
 		UDEBUG("");
 		UTimer time;
 		_octomap->update(poses);
-		_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
 		UINFO("Octomap update time = %fs", time.ticks());
 	}
+	if(_preferencesDialog->isOctomapShown())
+	{
+		UDEBUG("");
+		UTimer time;
+		_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
+		UINFO("Octomap show 3d map time = %fs", time.ticks());
+	}
 #endif

 	if(viewerClouds.contains("cloudOdom"))
@@ -2576,103 +2650,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 	return outputPair;
 }

-void MainWindow::createAndAddProjectionMap(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		int nodeId,
-		const Transform & pose,
-		bool updateOctomap)
-{
-	UDEBUG("");
-	UASSERT(!pose.isNull());
-
-	if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end() && !updateOctomap)
-	{
-		UERROR("Projection map %d already added.", nodeId);
-		return;
-	}
-
-	if(indices->size())
-	{
-		UTimer timer;
-		cv::Mat ground, obstacles;
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloud;
-
-		// voxelize to grid cell size
-		if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
-		{
-			voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
-		}
-
-		// add pose rotation without yaw
-		float roll, pitch, yaw;
-		pose.getEulerAngles(roll, pitch, yaw);
-		voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, _preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0));
-
-		if(_preferencesDialog->projMaxObstaclesHeight())
-		{
-			voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
-		}
-
-		pcl::IndicesPtr groundIndices, obstaclesIndices;
-		util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
-				voxelCloud,
-				groundIndices,
-				obstaclesIndices,
-				20,
-				_preferencesDialog->projMaxGroundAngle(),
-				_preferencesDialog->getGridMapResolution()*2.0f,
-				_preferencesDialog->projMinClusterSize(),
-				_preferencesDialog->projFlatObstaclesDetected(),
-				_preferencesDialog->projMaxGroundHeight());
-
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-		if(groundIndices->size())
-		{
-			pcl::copyPointCloud(*voxelCloud, *groundIndices, *groundCloud);
-		}
-
-		if(obstaclesIndices->size())
-		{
-			pcl::copyPointCloud(*voxelCloud, *obstaclesIndices, *obstaclesCloud);
-		}
-
-		util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-				groundCloud,
-				obstaclesCloud,
-				ground,
-				obstacles,
-				_preferencesDialog->getGridMapResolution());
-
-		if(updateOctomap)
-		{
-			// Update octomap
-#ifdef RTABMAP_OCTOMAP
-			if(_octomap->addedNodes().empty() ||
-				nodeId > _octomap->addedNodes().rbegin()->first)
-			{
-				Transform tinv = Transform(0,0,_preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0).inverse();
-				groundCloud = util3d::transformPointCloud(groundCloud, tinv);
-				obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
-
-				if(_preferencesDialog->isOctomapGroundAnObstacle())
-				{
-					*obstaclesCloud += *groundCloud;
-					groundCloud->clear();
-				}
-				_octomap->addToCache(nodeId, groundCloud, obstaclesCloud);
-			}
-#endif
-		}
-
-		_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-		UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
-	}
-	UDEBUG("");
-}
-
 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
 {
 	std::string scanName = uFormat("scan%d", nodeId);
@@ -2702,7 +2679,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		if(scan.channels() == 6)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloudNormal(scan);
+			cloud = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
 			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
@@ -2729,7 +2706,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		else
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloud(scan);
+			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
 			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
@@ -2758,13 +2735,6 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 					}
 				}
 				_createdScans.insert(std::make_pair(nodeId, scan));
-
-				if(scan.channels() == 2)
-				{
-					cv::Mat ground, obstacles;
-					util3d::occupancy2DFromLaserScan(scan, ground, obstacles, _preferencesDialog->getGridMapResolution());
-					_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-				}
 			}
 		}
 		_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
@@ -3340,6 +3310,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
 {
 	ULOGGER_DEBUG("");
+	_occupancyGrid->parseParameters(parameters);
 	if(parameters.size())
 	{
 		for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
@@ -4056,6 +4027,9 @@ void MainWindow::startDetection()
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif

+	_occupancyGrid->clear();
+	_occupancyGrid->parseParameters(parameters);
+
 	emit stateChanged(kDetecting);
 }

@@ -5023,7 +4997,7 @@ void MainWindow::clearTheCache()
 	_previousCloud.second.second.reset();
 	_createdScans.clear();
 	_gridLocalMaps.clear();
-	_projectionLocalMaps.clear();
+	_cachedGridsMemoryUsage = 0;
 	_createdFeatures.clear();
 	_cloudViewer->clear();
 	_cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
@@ -5072,6 +5046,7 @@ void MainWindow::clearTheCache()
 	delete _octomap;
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif
+	_occupancyGrid->clear();
 }

 void MainWindow::updateElapsedTime()
@@ -5325,9 +5300,9 @@ void MainWindow::setAspectRatioCustom()

 void MainWindow::exportGridMap()
 {
-	double gridCellSize = 0.05;
+	float gridCellSize = 0.05f;
 	bool ok;
-	gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
+	gridCellSize = (float)QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), (double)gridCellSize, 0.01, 1, 2, &ok);
 	if(!ok)
 	{
 		return;
@@ -5337,13 +5312,24 @@ void MainWindow::exportGridMap()

 	// create the map
 	float xMin=0.0f, yMin=0.0f;
-	cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
+	cv::Mat pixels;
+#ifdef RTABMAP_OCTOMAP
+		if(_preferencesDialog->isOctomap2dGrid())
+		{
+			pixels = _octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
+
+		}
+		else
+#endif
+		{
+			pixels = util3d::create2DMapFromOccupancyLocalMaps(
 				poses,
-				_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
+				_gridLocalMaps,
 				gridCellSize,
 				xMin, yMin,
 				0,
 				_preferencesDialog->isGridMapEroded());
+		}

 	if(!pixels.empty())
 	{
@@ -5946,7 +5932,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6007,7 +5993,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6130,7 +6116,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 			_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 			_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-			_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+			_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 			_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 			_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6195,7 +6181,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -378,17 +378,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_footprintRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->groupBox_map_occupancyFrom3DCloud, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_projMapFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxGroundAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxGroundHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->spinBox_projMinClusterSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxObstaclesHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_projFlatObstaclesDetected, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));

 	connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_octomap_groundObstacle, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));

 	connect(_ui->groupBox_organized, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -728,6 +722,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
+	_ui->checkbox_rgbd_createOccupancyGRid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());

 	// Registration
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
@@ -778,6 +773,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
 	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());

+	// Occupancy grid
+	_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
+	_ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGrid3DGroundIsObstacle().c_str());
+	_ui->doubleSpinBox_grid_resolution->setObjectName(Parameters::kGridCellSize().c_str());
+	_ui->spinBox_grid_decimation->setObjectName(Parameters::kGridDepthDecimation().c_str());
+	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
+	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
+	_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
+	_ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str());
+	_ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str());
+	_ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str());
+	_ui->doubleSpinBox_grid_maxGroundAngle->setObjectName(Parameters::kGridMaxGroundAngle().c_str());
+	_ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str());
+	_ui->doubleSpinBox_grid_maxGroundHeight->setObjectName(Parameters::kGridMaxGroundHeight().c_str());
+	_ui->doubleSpinBox_grid_maxObstacleHeight->setObjectName(Parameters::kGridMaxObstacleHeight().c_str());
+	_ui->spinBox_grid_minClusterSize->setObjectName(Parameters::kGridMinClusterSize().c_str());
+	_ui->doubleSpinBox_grid_minGroundHeight->setObjectName(Parameters::kGridMinGroundHeight().c_str());
+	_ui->spinBox_grid_noiseMinNeighbors->setObjectName(Parameters::kGridNoiseFilteringMinNeighbors().c_str());
+	_ui->doubleSpinBox_grid_noiseRadius->setObjectName(Parameters::kGridNoiseFilteringRadius().c_str());
+	_ui->groupBox_grid_normalsSegmentation->setObjectName(Parameters::kGridNormalsSegmentation().c_str());
+	_ui->checkBox_grid_unknownSpaceFilled->setObjectName(Parameters::kGridScan2dUnknownSpaceFilled().c_str());
+	_ui->doubleSpinBox_grid_unknownSpaceFilledMaxRange->setObjectName(Parameters::kGridScan2dMaxFilledRange().c_str());
+	_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());

 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
@@ -1221,19 +1239,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_erode->setChecked(false);
+		_ui->checkBox_map_incremental->setChecked(false);
 		_ui->doubleSpinBox_map_footprintRadius->setValue(0);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
-		_ui->groupBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->checkBox_projMapFrame->setChecked(true);
-		_ui->doubleSpinBox_projMaxGroundAngle->setValue(30);
-		_ui->doubleSpinBox_projMaxGroundHeight->setValue(0);
-		_ui->spinBox_projMinClusterSize->setValue(20);
-		_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(0);
-		_ui->checkBox_projFlatObstaclesDetected->setChecked(true);

 		_ui->groupBox_octomap->setChecked(false);
 		_ui->spinBox_octomap_treeDepth->setValue(16);
-		_ui->checkBox_octomap_groundObstacle->setChecked(true);
+		_ui->checkBox_octomap_2dgrid->setChecked(true);
+		_ui->checkBox_octomap_show3dMap->setChecked(true);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
 	{
@@ -1577,19 +1590,14 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
 	_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
+	_ui->checkBox_map_incremental->setChecked(settings.value("gridMapIncremental", _ui->checkBox_map_incremental->isChecked()).toBool());
 	_ui->doubleSpinBox_map_footprintRadius->setValue(settings.value("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()).toDouble());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
-	_ui->groupBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()).toBool());
-	_ui->checkBox_projMapFrame->setChecked(settings.value("projMapFrame", _ui->checkBox_projMapFrame->isChecked()).toBool());
-	_ui->doubleSpinBox_projMaxGroundAngle->setValue(settings.value("projMaxGroundAngle", _ui->doubleSpinBox_projMaxGroundAngle->value()).toDouble());
-	_ui->doubleSpinBox_projMaxGroundHeight->setValue(settings.value("projMaxGroundHeight", _ui->doubleSpinBox_projMaxGroundHeight->value()).toDouble());
-	_ui->spinBox_projMinClusterSize->setValue(settings.value("projMinClusterSize", _ui->spinBox_projMinClusterSize->value()).toInt());
-	_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(settings.value("projMaxObstaclesHeight", _ui->doubleSpinBox_projMaxObstaclesHeight->value()).toDouble());
-	_ui->checkBox_projFlatObstaclesDetected->setChecked(settings.value("projFlatObstaclesDetected", _ui->checkBox_projFlatObstaclesDetected->isChecked()).toBool());

 	_ui->groupBox_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());
-	_ui->checkBox_octomap_groundObstacle->setChecked(settings.value("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked()).toBool());
+	_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
+	_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());

 	_ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool());
 	_ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble());
@@ -1994,20 +2002,14 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("gridMapShown",                _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution",           _ui->doubleSpinBox_map_resolution->value());
 	settings.setValue("gridMapEroded",               _ui->checkBox_map_erode->isChecked());
+	settings.setValue("gridMapIncremental",          _ui->checkBox_map_incremental->isChecked());
 	settings.setValue("gridMapFootprintRadius",      _ui->doubleSpinBox_map_footprintRadius->value());
 	settings.setValue("gridMapOpacity",              _ui->doubleSpinBox_map_opacity->value());

-	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked());
-	settings.setValue("projMapFrame",                _ui->checkBox_projMapFrame->isChecked());
-	settings.setValue("projMaxGroundAngle",          _ui->doubleSpinBox_projMaxGroundAngle->value());
-	settings.setValue("projMaxGroundHeight",         _ui->doubleSpinBox_projMaxGroundHeight->value());
-	settings.setValue("projMinClusterSize",          _ui->spinBox_projMinClusterSize->value());
-	settings.setValue("projMaxObstaclesHeight",      _ui->doubleSpinBox_projMaxObstaclesHeight->value());
-	settings.setValue("projFlatObstaclesDetected",   _ui->checkBox_projFlatObstaclesDetected->isChecked());
-
 	settings.setValue("octomap",                     _ui->groupBox_octomap->isChecked());
 	settings.setValue("octomap_depth",               _ui->spinBox_octomap_treeDepth->value());
-	settings.setValue("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked());
+	settings.setValue("octomap_2dgrid",              _ui->checkBox_octomap_2dgrid->isChecked());
+	settings.setValue("octomap_3dmap",               _ui->checkBox_octomap_show3dMap->isChecked());

 	settings.setValue("meshing",               _ui->groupBox_organized->isChecked());
 	settings.setValue("meshing_angle",         _ui->doubleSpinBox_mesh_angleTolerance->value());
@@ -2659,9 +2661,10 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri

 rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
 {
-	UASSERT_MSG(_parameters.size() == Parameters::getDefaultParameters().size(),
-			uFormat("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size()).c_str());
-
+	if(_parameters.size() != Parameters::getDefaultParameters().size())
+	{
+		UWARN("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size());
+	}
 	ParametersMap parameters = _parameters;
 	uInsert(parameters, _modifiedParameters);

@@ -3687,20 +3690,34 @@ bool PreferencesDialog::isCloudsShown(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingShowClouds[index]->isChecked();
 }
-bool PreferencesDialog::isOctomapShown() const
+bool PreferencesDialog::isOctomapUpdated() const
 {
 #ifdef RTABMAP_OCTOMAP
 	return _ui->groupBox_octomap->isChecked();
 #endif
 	return false;
 }
+bool PreferencesDialog::isOctomapShown() const
+{
+#ifdef RTABMAP_OCTOMAP
+	return _ui->groupBox_octomap->isChecked() && _ui->checkBox_octomap_show3dMap->isChecked();
+#endif
+	return false;
+}
+bool PreferencesDialog::isOctomap2dGrid() const
+{
+#ifdef RTABMAP_OCTOMAP
+	return _ui->groupBox_octomap->isChecked() && _ui->checkBox_octomap_2dgrid->isChecked();
+#endif
+	return false;
+}
 int PreferencesDialog::getOctomapTreeDepth() const
 {
 	return _ui->spinBox_octomap_treeDepth->value();
 }
 bool PreferencesDialog::isOctomapGroundAnObstacle() const
 {
-	return _ui->checkBox_octomap_groundObstacle->isChecked();
+	return _ui->checkBox_grid_groundObstacle->isChecked();
 }

 double PreferencesDialog::getMapVoxel() const
@@ -3847,37 +3864,41 @@ bool PreferencesDialog::isGridMapEroded() const
 {
 	return _ui->checkBox_map_erode->isChecked();
 }
+bool PreferencesDialog::isGridMapIncremental() const
+{
+	return _ui->checkBox_map_incremental->isChecked();
+}
 double PreferencesDialog::getGridMapFootprintRadius() const
 {
 	return _ui->doubleSpinBox_map_footprintRadius->value();
 }
 bool PreferencesDialog::isGridMapFrom3DCloud() const
 {
-	return _ui->groupBox_map_occupancyFrom3DCloud->isChecked();
+	return _ui->groupBox_grid_fromDepthImage->isChecked();
 }
 bool PreferencesDialog::projMapFrame() const
 {
-	return _ui->checkBox_projMapFrame->isChecked();
+	return _ui->checkBox_grid_projMapFrame->isChecked();
 }
 double PreferencesDialog::projMaxGroundAngle() const
 {
-	return _ui->doubleSpinBox_projMaxGroundAngle->value()*M_PI/180.0;
+	return _ui->doubleSpinBox_grid_maxGroundAngle->value()*M_PI/180.0;
 }
 double PreferencesDialog::projMaxGroundHeight() const
 {
-	return _ui->doubleSpinBox_projMaxGroundHeight->value();
+	return _ui->doubleSpinBox_grid_maxGroundHeight->value();
 }
 int PreferencesDialog::projMinClusterSize() const
 {
-	return _ui->spinBox_projMinClusterSize->value();
+	return _ui->spinBox_grid_minClusterSize->value();
 }
 double PreferencesDialog::projMaxObstaclesHeight() const
 {
-	return _ui->doubleSpinBox_projMaxObstaclesHeight->value();
+	return _ui->doubleSpinBox_grid_maxObstacleHeight->value();
 }
 bool PreferencesDialog::projFlatObstaclesDetected() const
 {
-	return _ui->checkBox_projFlatObstaclesDetected->isChecked();
+	return _ui->checkBox_grid_flatObstaclesDetected->isChecked();
 }
 double PreferencesDialog::getGridMapOpacity() const
 {
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -52,7 +52,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>198</width>
+           <width>202</width>
           <height>196</height>
          </rect>
         </property>
@@ -210,7 +210,7 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>197</width>
+           <width>201</width>
           <height>196</height>
          </rect>
         </property>
@@ -980,7 +980,7 @@
     <item>
      <widget class="QToolBox" name="toolBox">
       <property name="currentIndex">
-        <number>1</number>
+        <number>3</number>
       </property>
       <widget class="QWidget" name="page_3">
        <property name="geometry">
@@ -1122,8 +1122,8 @@
        <property name="geometry">
         <rect>
          <x>0</x>
-          <y>-245</y>
-          <width>284</width>
+          <y>0</y>
+          <width>278</width>
          <height>611</height>
         </rect>
        </property>
@@ -1634,8 +1634,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>175</width>
-          <height>191</height>
+          <width>289</width>
+          <height>182</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1673,7 +1673,7 @@
          </widget>
         </item>
         <item row="0" column="0">
-          <widget class="QCheckBox" name="checkBox_icp_laserScan">
+          <widget class="QCheckBox" name="checkBox_icp_from_depth">
           <property name="text">
            <string/>
           </property>
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui