0.16.0: Database updated with Data.empty_cells, Admin.opt_map, Admin.opt_map_x_min and Admin.opt_map_y_min fields. Changed Parameter Grid/ProjRayTracing to Grid/RayTracing (OctoMap ray tracing done for 3D local grids). Improved OctoMap performance.

2026-09-02 17:40:23 +08:00 · 2018-02-08 21:40:17 -05:00
parent e7ceacc215
commit fced2c521c
32 changed files with 1642 additions and 600 deletions
--- a/corelib/src/DBDriver.cpp
+++ b/corelib/src/DBDriver.cpp
@@ -475,17 +475,19 @@ void DBDriver::updateOccupancyGrid(
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		float cellSize,
 		const cv::Point3f & viewpoint)
 {
 	_dbSafeAccessMutex.lock();
 	//just to make sure the occupancy grids are compressed for convenience
 	SensorData data;
-	data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
+	data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewpoint);
 	this->updateOccupancyGridQuery(
 			nodeId,
 			data.gridGroundCellsCompressed(),
 			data.gridObstacleCellsCompressed(),
+			data.gridEmptyCellsCompressed(),
 			cellSize,
 			viewpoint);
 	_dbSafeAccessMutex.unlock();
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -1220,7 +1220,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			}
 			if(occupancyGrid)
 			{
-				fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				if(uStrNumCmp(_version, "0.16.0") >= 0)
+				{
+					fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				}
+				else
+				{
+					fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				}
 			}

 			query << "SELECT " << fields.str().c_str() << " "
@@ -1557,6 +1564,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				// Occupancy grid
 				cv::Mat groundCellsCompressed;
 				cv::Mat obstacleCellsCompressed;
+				cv::Mat emptyCellsCompressed;
 				float cellSize = 0.0f;
 				cv::Point3f viewPoint;
 				if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
@@ -1579,6 +1587,18 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
 					}

+					if(uStrNumCmp(_version, "0.16.0") >= 0)
+					{
+						// empty
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if(dataSize > 0 && data)
+						{
+							emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
+							memcpy((void*)emptyCellsCompressed.data, data, dataSize);
+						}
+					}
+
 					cellSize = sqlite3_column_double(ppStmt, index++);
 					viewPoint.x = sqlite3_column_double(ppStmt, index++);
 					viewPoint.y = sqlite3_column_double(ppStmt, index++);
@@ -1612,11 +1632,11 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				}
 				if(occupancyGrid)
 				{
-					(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, cellSize, viewPoint);
+					(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
 				}
 				else
 				{
-					(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
+					(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridEmptyCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
 				}
 				rc = sqlite3_step(ppStmt); // next result...
 			}
@@ -3881,6 +3901,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const
 {
@@ -3903,6 +3924,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 				nodeId,
 				ground,
 				obstacles,
+				empty,
 				cellSize,
 				viewpoint);

@@ -4834,7 +4856,11 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.11.10") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -5046,6 +5072,21 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		}

+		if(uStrNumCmp(_version, "0.16.0") >= 0)
+		{
+			//empty_cells
+			if(sensorData.gridEmptyCellsCompressed().empty())
+			{
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			else
+			{
+				rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+		}
+
 		//cell_size
 		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -5308,22 +5349,29 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
+	}
 	return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
 }
 void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		float cellSize,
 		const cv::Point3f & viewpoint) const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
 	UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
 	UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
-	UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d cell=%f viewpoint=(%f,%f,%f)",
+	UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
+	UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
 			nodeId,
 			ground.cols,
 			obstacles.cols,
+			empty.cols,
 			cellSize,
 			viewpoint.x,
 			viewpoint.y,
@@ -5361,6 +5409,21 @@ void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 	}

+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		//empty_cells
+		if(empty.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
 	//cell_size
 	rc = sqlite3_bind_double(ppStmt, index++, cellSize);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
--- a/corelib/src/DBDriverSqlite3.h
+++ b/corelib/src/DBDriverSqlite3.h
@@ -89,6 +89,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;

@@ -163,6 +164,7 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;

--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -4043,19 +4043,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}

 	// Occupancy grid map stuff
-	cv::Mat ground, obstacles;
+	cv::Mat ground, obstacles, empty;
 	float cellSize = 0.0f;
 	cv::Point3f viewPoint(0,0,0);
 	if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
 	{
-		_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
+		_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
 		cellSize = _occupancy->getCellSize();

 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
 		UDEBUG("time grid map = %fs", t);
 	}
-	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
+	s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);

 	// prior
 	if(!isIntermediateNode)
--- a/corelib/src/OccupancyGrid.cpp
+++ b/corelib/src/OccupancyGrid.cpp
@@ -32,6 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>

+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#endif
+
 #include <pcl/io/pcd_io.h>

 namespace rtabmap {
@@ -64,16 +68,18 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
 	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
-	projRayTracing_(Parameters::defaultGridProjRayTracing()),
+	rayTracing_(Parameters::defaultGridRayTracing()),
 	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize()),
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
+	updateError_(Parameters::defaultGridGlobalUpdateError()),
 	xMin_(0.0f),
 	yMin_(0.0f),
 	cloudAssembling_(false),
 	assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
-	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>)
+	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
 {
 	this->parseParameters(parameters);
 }
@@ -117,11 +123,12 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
 	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
-	Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
+	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
 	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);

 	UASSERT(minMapSize_ >= 0.0f);

@@ -213,8 +220,9 @@ void OccupancyGrid::setCloudAssembling(bool enabled)

 void OccupancyGrid::createLocalMap(
 		const Signature & node,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
 		cv::Point3f & viewPoint) const
 {
 	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
@@ -233,11 +241,13 @@ void OccupancyGrid::createLocalMap(
 				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
 				cv::Mat(),
 				viewPoint,
-				ground,
-				obstacles,
+				emptyCells,
+				obstacleCells,
 				cellSize_,
 				scan2dUnknownSpaceFilled_,
 				node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
+
+		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
 	}
 	else
 	{
@@ -300,94 +310,149 @@ void OccupancyGrid::createLocalMap(
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
 		}
+		createLocalMap(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+	}
+}

-		if(projMapFrame_)
+void OccupancyGrid::createLocalMap(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	UASSERT_MSG(cloud->size() && cloud->is_dense, uFormat("Use interface with indices if cloud is not dense.").c_str());
+	createLocalMap(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
+}
+
+void OccupancyGrid::createLocalMap(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+		const pcl::IndicesPtr & indices,
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	if(projMapFrame_)
+	{
+		//we should rotate viewPoint in /map frame
+		float roll, pitch, yaw;
+		pose.getEulerAngles(roll, pitch, yaw);
+		Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
+		viewPointInOut.x = viewpointRotated.x();
+		viewPointInOut.y = viewpointRotated.y();
+		viewPointInOut.z = viewpointRotated.z();
+	}
+
+	if((cloud->is_dense && cloud->size()) ||
+		(!cloud->is_dense && indices->size()))
+	{
+		pcl::IndicesPtr groundIndices(new std::vector<int>);
+		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = this->segmentCloud<pcl::PointXYZRGB>(
+				cloud,
+				indices,
+				pose,
+				viewPointInOut,
+				groundIndices,
+				obstaclesIndices);
+
+		if(!groundIndices->empty() || !obstaclesIndices->empty())
 		{
-			//we should rotate viewPoint in /map frame
-			float roll, pitch, yaw;
-			node.getPose().getEulerAngles(roll, pitch, yaw);
-			Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPoint.x, viewPoint.y, viewPoint.z, 0,0,0);
-			viewPoint.x = viewpointRotated.x();
-			viewPoint.y = viewpointRotated.y();
-			viewPoint.z = viewpointRotated.z();
-		}
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);

-		if((cloud->is_dense && cloud->size()) ||
-			(!cloud->is_dense && indices->size()))
-		{
-			pcl::IndicesPtr groundIndices(new std::vector<int>);
-			pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-			cloud = this->segmentCloud<pcl::PointXYZRGB>(
-					cloud,
-					indices,
-					node.getPose(),
-					viewPoint,
-					groundIndices,
-					obstaclesIndices);
-
-			if(!groundIndices->empty() || !obstaclesIndices->empty())
+			if(groundIndices->size())
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				pcl::copyPointCloud(*cloudSegmented, *groundIndices, *groundCloud);
+			}

-				if(groundIndices->size())
+			if(obstaclesIndices->size())
+			{
+				pcl::copyPointCloud(*cloudSegmented, *obstaclesIndices, *obstaclesCloud);
+			}
+
+			if(grid3D_)
+			{
+				UDEBUG("");
+				if(groundIsObstacle_)
 				{
-					pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+					*obstaclesCloud += *groundCloud;
+					groundCloud->clear();
 				}

-				if(obstaclesIndices->size())
-				{
-					pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-				}
+				// transform back in base frame
+				float roll, pitch, yaw;
+				pose.getEulerAngles(roll, pitch, yaw);
+				Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();

-				if(grid3D_)
+				if(rayTracing_)
 				{
-					UDEBUG("");
-					if(groundIsObstacle_)
+#ifdef RTABMAP_OCTOMAP
+					if(!groundCloud->empty() || !obstaclesCloud->empty())
 					{
-						*obstaclesCloud += *groundCloud;
-						groundCloud->clear();
-					}
+						//create local octomap
+						OctoMap octomap(cellSize_);
+						octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
+						std::map<int, Transform> poses;
+						poses.insert(std::make_pair(1, Transform::getIdentity()));
+						octomap.update(poses);

-					// transform back in base frame
-					float roll, pitch, yaw;
-					node.getPose().getEulerAngles(roll, pitch, yaw);
-					Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
-					ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
-					obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+						obstaclesIndices->clear();
+						groundIndices->clear();
+						pcl::IndicesPtr emptyIndices(new std::vector<int>);
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
+						UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
+					}
 				}
 				else
+#else
+					UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
+				}
+#endif
 				{
-					UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
-					// projection on the xy plane
-					util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-							groundCloud,
-							obstaclesCloud,
-							ground,
-							obstacles,
-							cellSize_);
+					groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
+					obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+				}

-					if(projRayTracing_)
-					{
-						cv::Mat laserScan = obstacles;
-						cv::Mat laserScanNoHit = ground;
-						obstacles = cv::Mat();
-						ground = cv::Mat();
-						util3d::occupancy2DFromLaserScan(
-								laserScan,
-								laserScanNoHit,
-								viewPoint,
-								ground,
-								obstacles,
-								cellSize_,
-								false, // don't fill unknown space
-								0);
-					}
+			}
+			else
+			{
+				UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+				// projection on the xy plane
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+						groundCloud,
+						obstaclesCloud,
+						groundCells,
+						obstacleCells,
+						cellSize_);
+
+				if(rayTracing_)
+				{
+					cv::Mat laserScan = obstacleCells;
+					cv::Mat laserScanNoHit = groundCells;
+					obstacleCells = cv::Mat();
+					groundCells = cv::Mat();
+					util3d::occupancy2DFromLaserScan(
+							laserScan,
+							laserScanNoHit,
+							viewPointInOut,
+							emptyCells,
+							obstacleCells,
+							cellSize_,
+							false, // don't fill unknown space
+							0);
 				}
 			}
 		}
 	}
-	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
+	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
 }

 void OccupancyGrid::clear()
@@ -417,10 +482,11 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 void OccupancyGrid::addToCache(
 		int nodeId,
 		const cv::Mat & ground,
-		const cv::Mat & obstacles)
+		const cv::Mat & obstacles,
+		const cv::Mat & empty)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
 }

 void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
@@ -442,6 +508,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	std::map<int, Transform> transforms;
+	float updateErrorSqrd = updateError_*updateError_;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
@@ -451,7 +518,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
 			{
 				t = jter->second * iter->second.inverse();
 				graphOptimized = true;
@@ -487,6 +554,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 	bool assembledGroundUpdated = false;
 	bool assembledObstaclesUpdated = false;
+	bool assembledEmptyCellsUpdated = false;

 	if(graphOptimized || graphChanged)
 	{
@@ -676,22 +744,22 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		{
 			if(uContains(cache_, iter->first))
 			{
-				const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
+				const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);

 				//ground
-				if(pair.first.cols)
+				if(pair.first.first.cols)
 				{
-					if(pair.first.rows > 1 && pair.first.cols == 1)
+					if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 					}
-					cv::Mat ground(1, pair.first.cols, CV_32FC2);
+					cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
 					for(int i=0; i<ground.cols; ++i)
 					{
-						const float * vi = pair.first.ptr<float>(0,i);
+						const float * vi = pair.first.first.ptr<float>(0,i);
 						float * vo = ground.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() != 2 && pair.first.channels() != 5)
+						if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -715,25 +783,67 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 					if(cloudAssembling_)
 					{
-						*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first, iter->second, 0, 255, 0);
+						*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first.first, iter->second, 0, 255, 0);
 						assembledGroundUpdated = true;
 					}
 				}

-				//obstacles
+				//empty
 				if(pair.second.cols)
 				{
 					if(pair.second.rows > 1 && pair.second.cols == 1)
 					{
 						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 					}
-					cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
-					for(int i=0; i<obstacles.cols; ++i)
+					cv::Mat ground(1, pair.second.cols, CV_32FC2);
+					for(int i=0; i<ground.cols; ++i)
 					{
 						const float * vi = pair.second.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.second.channels() != 2 && pair.second.channels() != 5)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+
+					if(cloudAssembling_)
+					{
+						*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 0, 255, 0);
+						assembledEmptyCellsUpdated = true;
+					}
+				}
+
+				//obstacles
+				if(pair.first.second.cols)
+				{
+					if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
+					}
+					cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = pair.first.second.ptr<float>(0,i);
 						float * vo = obstacles.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() != 2 && pair.first.channels() != 5)
+						if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -757,7 +867,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)

 					if(cloudAssembling_)
 					{
-						*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 255, 0, 0);
+						*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.first.second, iter->second, 255, 0, 0);
 						assembledObstaclesUpdated = true;
 					}
 				}
@@ -1165,6 +1275,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		{
 			assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
 		}
+		if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
+		{
+			assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
+		}
 	}

 	if(!fullUpdate_ && !cloudAssembling_)
@@ -1174,7 +1288,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	else
 	{
 		//clear only negative ids
-		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
 		{
 			if(iter->first < 0)
 			{
--- a/corelib/src/OctoMap.cpp
+++ b/corelib/src/OctoMap.cpp
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OctoMap.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
@@ -35,24 +36,159 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

+//////////////////////////////////////
+// RtabmapColorOcTree
+//////////////////////////////////////
+
+RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
+	: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
+	RtabmapColorOcTreeMemberInit.ensureLinking();
+};
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search (key);
+	if (n != 0) {
+		n->setColor(r, g, b);
+	}
+	return n;
+}
+
+bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
+	if (!isNodeCollapsible(node))
+		return false;
+
+	// set value to children's values (all assumed equal)
+	node->copyData(*(getNodeChild(node, 0)));
+
+	if (node->isColorSet()) // TODO check
+		node->setColor(node->getAverageChildColor());
+
+	// delete children
+	for (unsigned int i=0;i<8;i++) {
+		deleteNodeChild(node, i);
+	}
+	delete[] node->children;
+	node->children = NULL;
+
+	return true;
+}
+
+bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
+	// all children must exist, must not have children of
+	// their own and have the same occupancy probability
+	if (!nodeChildExists(node, 0))
+		return false;
+
+	const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
+	if (nodeHasChildren(firstChild))
+		return false;
+
+	for (unsigned int i = 1; i<8; i++) {
+		// compare nodes only using their occupancy, ignoring color for pruning
+		if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
+			return false;
+	}
+
+	return true;
+}
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search(key);
+	if (n != 0) {
+		if (n->isColorSet()) {
+			RtabmapColorOcTreeNode::Color prev_color = n->getColor();
+			n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
+		}
+		else {
+			n->setColor(r, g, b);
+		}
+	}
+	return n;
+}
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search (key);
+	if (n != 0) {
+		if (n->isColorSet()) {
+			RtabmapColorOcTreeNode::Color prev_color = n->getColor();
+			double node_prob = n->getOccupancy();
+			uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
+					+  (double) r * (0.99-node_prob));
+			uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
+					+  (double) g * (0.99-node_prob));
+			uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
+					+  (double) b * (0.99-node_prob));
+			n->setColor(new_r, new_g, new_b);
+		}
+		else {
+			n->setColor(r, g, b);
+		}
+	}
+	return n;
+}
+
+
+void RtabmapColorOcTree::updateInnerOccupancy() {
+	this->updateInnerOccupancyRecurs(this->root, 0);
+}
+
+void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
+	// only recurse and update for inner nodes:
+	if (nodeHasChildren(node)){
+		// return early for last level:
+		if (depth < this->tree_depth){
+			for (unsigned int i=0; i<8; i++) {
+				if (nodeChildExists(node, i)) {
+					updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
+				}
+			}
+		}
+		node->updateOccupancyChildren();
+		node->updateColorChildren();
+	}
+}
+
+
+//////////////////////////////////////
+// OctoMap
+//////////////////////////////////////
+
 OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
 		hasColor_(false),
-		fullUpdate_(Parameters::defaultGridGlobalFullUpdate())
+		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
+		updateError_(Parameters::defaultGridGlobalUpdateError())
 {
 	float cellSize = Parameters::defaultGridCellSize();
 	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
 	UASSERT(cellSize>0.0f);

-	octree_ = new octomap::ColorOcTree(cellSize);
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
+	octree_ = new RtabmapColorOcTree(cellSize);
 	octree_->setOccupancyThres(occupancyThr);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 }

-OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate) :
-		octree_(new octomap::ColorOcTree(cellSize)),
+OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
+		octree_(new RtabmapColorOcTree(cellSize)),
 		hasColor_(false),
-		fullUpdate_(fullUpdate)
+		fullUpdate_(fullUpdate),
+		updateError_(updateError)
 {
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
 	octree_->setOccupancyThres(occupancyThr);
 	UASSERT(cellSize>0.0f);
 }
@@ -66,13 +202,14 @@ OctoMap::~OctoMap()
 void OctoMap::clear()
 {
 	octree_->clear();
-	occupiedCells_.clear();
 	cache_.clear();
 	cacheClouds_.clear();
 	cacheViewPoints_.clear();
 	addedNodes_.clear();
 	keyRay_ = octomap::KeyRay();
 	hasColor_ = false;
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
 }

 void OctoMap::addToCache(int nodeId,
@@ -87,12 +224,14 @@ void OctoMap::addToCache(int nodeId,
 void OctoMap::addToCache(int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		const cv::Point3f & viewPoint)
 {
-	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
-	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
+	UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
+	UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
+	UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
 	uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
 }

@@ -105,6 +244,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	std::map<int, Transform> transforms;
 	std::map<int, Transform> updatedAddedNodes;
+	float updateErrorSqrd = updateError_*updateError_;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
@@ -113,7 +253,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			graphChanged = false;
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
 			{
 				t = jter->second * iter->second.inverse();
 				graphOptimized = true;
@@ -137,64 +277,99 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			UINFO("Graph optimized!");
 		}

+		minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+		maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
 		if(fullUpdate_ || graphChanged)
 		{
 			// clear all but keep cache
 			octree_->clear();
-			occupiedCells_.clear();
 			addedNodes_.clear();
 			keyRay_ = octomap::KeyRay();
 			hasColor_ = false;
 		}
 		else
 		{
-			octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
-			std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
+			RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
 			int copied=0;
-			for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
-					iter!=occupiedCells_.end();
-					++iter)
+			int count=0;
+			UTimer t;
+			for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
 			{
-				std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
-				if(jter != transforms.end())
+				RtabmapColorOcTreeNode & nOld = *it;
+				if(nOld.getNodeRefId() > 0)
 				{
-					octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
-					std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
-					UASSERT(pter != addedNodes_.end());
-
-					cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
-					cvPt = util3d::transformPoint(cvPt, jter->second);
-
-					octomap::OcTreeKey key;
-					if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
+					std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
+					if(jter != transforms.end())
 					{
-						octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
-						if(n)
+						octomap::point3d pt;
+						std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
+						UASSERT(pter != addedNodes_.end());
+
+						if(nOld.getOccupancyType() > 0)
 						{
-							++copied;
-							uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
-							newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+							pt = nOld.getPointRef();
 						}
 						else
 						{
-							UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+							pt = octree_->keyToCoord(it.getKey());
+						}
+
+						cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+						cvPt = util3d::transformPoint(cvPt, jter->second);
+						octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
+
+						octomap::OcTreeKey key;
+						if(newOcTree->coordToKeyChecked(ptTransformed, key))
+						{
+							RtabmapColorOcTreeNode * n = newOcTree->search(key);
+							if(n)
+							{
+								if(n->getNodeRefId() > nOld.getNodeRefId())
+								{
+									// The cell has been updated from more recent node, don't update the cell
+									continue;
+								}
+								else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
+								{
+									// empty cells cannot overwrite ground/obstacle cells
+									continue;
+								}
+							}
+
+							RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
+							if(nNew)
+							{
+								++copied;
+								updateMinMax(ptTransformed);
+								nNew->setNodeRefId(nOld.getNodeRefId());
+								if(nOld.getOccupancyType() > 0)
+								{
+									nNew->setPointRef(pt);
+								}
+								nNew->setOccupancyType(nOld.getOccupancyType());
+								nNew->setColor(nOld.getColor());
+							}
+							else
+							{
+								UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+							}
+						}
+						else
+						{
+							UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
 						}
 					}
-					else
+					else if(jter == transforms.end())
 					{
-						UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						// Note: normal if old nodes were transfered to LTM
+						//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
 					}
 				}
-				else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
-				{
-					// Note: normal if old nodes were transfered to LTM
-					//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
-				}
 			}
-			UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
+			UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
 			delete octree_;
 			octree_ = newOcTree;
-			occupiedCells_ = newOccupiedCells;

 			//update added poses
 			addedNodes_ = updatedAddedNodes;
@@ -235,7 +410,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 	{
 		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-		std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
+		std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
 		std::map<int, cv::Point3f>::iterator viewPointIter;
 		cloudIter = cacheClouds_.find(iter->first);
 		occupancyIter = cache_.find(iter->first);
@@ -248,6 +423,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
 			sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);

+			updateMinMax(sensorOrigin);
+
 			octomap::OcTreeKey tmpKey;
 			if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
 					|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -255,10 +432,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 			}

+			bool computeRays = occupancyIter == cache_.end() || occupancyIter->second.second.empty();
+
 			// instead of direct scan insertion, compute update to filter ground:
-			octomap::KeySet free_cells, occupied_cells, ground_cells;
+			octomap::KeySet free_cells;
 			// insert ground points only as free:
-			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
+			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
 			UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
 			Eigen::Affine3f t = iter->second.toEigen3f();
 			for (unsigned int i=0; i<maxGroundPts; ++i)
@@ -266,7 +445,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				pcl::PointXYZRGB pt;
 				if(occupancyIter != cache_.end())
 				{
-					pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first.first, i);
 					pt = pcl::transformPoint(pt, t);
 				}
 				else
@@ -277,7 +456,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				octomap::point3d point(pt.x, pt.y, pt.z);

 				// only clear space (ground points)
-				if ((iter->first < 0 || iter->first>lastId) &&
+				if (computeRays &&
+					(iter->first < 0 || iter->first>lastId) &&
 					octree_->computeRayKeys(sensorOrigin, point, keyRay_))
 				{
 					free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -288,17 +468,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				{
 					if(iter->first >0 && iter->first<lastId)
 					{
-						octomap::ColorOcTreeNode * n = octree_->search(key);
-						if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
+						RtabmapColorOcTreeNode * n = octree_->search(key);
+						if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
 						{
 							// The cell has been updated from more recent node, don't update the cell
 							continue;
 						}
 					}

-					ground_cells.insert(key);
+					updateMinMax(point);

-					octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
 					if(n)
 					{
 						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -308,26 +488,24 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 						if(iter->first > 0)
 						{
-							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-						}
-						else
-						{
-							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+							n->setNodeRefId(iter->first);
+							n->setPointRef(point);
 						}
+						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
 					}
 				}
 			}
-			UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
+			UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());

 			// all other points: free on ray, occupied on endpoint:
-			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
+			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
 			UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
 			for (unsigned int i=0; i<maxObstaclePts; ++i)
 			{
 				pcl::PointXYZRGB pt;
 				if(occupancyIter != cache_.end())
 				{
-					pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first.second, i);
 					pt = pcl::transformPoint(pt, t);
 				}
 				else
@@ -338,7 +516,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				octomap::point3d point(pt.x, pt.y, pt.z);

 				// free cells
-				if ((iter->first < 0 || iter->first>lastId) &&
+				if (computeRays &&
+					(iter->first < 0 || iter->first>lastId) &&
 					octree_->computeRayKeys(sensorOrigin, point, keyRay_))
 				{
 					free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -349,17 +528,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				{
 					if(iter->first >0 && iter->first<lastId)
 					{
-						octomap::ColorOcTreeNode * n = octree_->search(key);
-						if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
+						RtabmapColorOcTreeNode * n = octree_->search(key);
+						if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
 						{
 							// The cell has been updated from more recent node, don't update the cell
 							continue;
 						}
 					}

-					occupied_cells.insert(key);
+					updateMinMax(point);

-					octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
 					if(n)
 					{
 						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -369,32 +548,60 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 						if(iter->first > 0)
 						{
-							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-						}
-						else
-						{
-							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+							n->setNodeRefId(iter->first);
+							n->setPointRef(point);
 						}
+						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
 					}
 				}
 			}
-			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
+			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());


 			// mark free cells only if not seen occupied in this cloud
 			for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
 			{
-				if (occupied_cells.find(*it) == occupied_cells.end() &&
-					ground_cells.find(*it) == ground_cells.end())
+				RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false);
+				if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 				{
-					octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
-					if(n)
+					n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
+					n->setNodeRefId(iter->first);
+				}
+			}
+
+			// all empty cells
+			if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
+			{
+				unsigned int maxEmptyPts = occupancyIter->second.second.cols;
+				UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
+				for (unsigned int i=0; i<maxEmptyPts; ++i)
+				{
+					pcl::PointXYZ pt;
+					pt = util3d::laserScanToPoint(occupancyIter->second.second, i);
+					pt = pcl::transformPoint(pt, t);
+
+					octomap::point3d point(pt.x, pt.y, pt.z);
+
+					octomap::OcTreeKey key;
+					if (octree_->coordToKeyChecked(point, key))
 					{
-						std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
-						gter = occupiedCells_.find(n);
-						if(gter != occupiedCells_.end() && gter->second.isObstacle_)
+						updateMinMax(point);
+
+						if(iter->first >0 && iter->first<lastId)
 						{
-							occupiedCells_.erase(gter);
+							RtabmapColorOcTreeNode * n = octree_->search(key);
+							if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
+							{
+								// The cell has been updated from more recent node, don't update the cell
+								continue;
+							}
+						}
+
+						RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
+						if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
+						{
+							n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
+							n->setNodeRefId(iter->first);
 						}
 					}
 				}
@@ -415,6 +622,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			UDEBUG("Did not find %d in cache", iter->first);
 		}
 	}
+
 	if(!fullUpdate_)
 	{
 		cache_.clear();
@@ -423,7 +631,35 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	}
 }

-void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
+void OctoMap::updateMinMax(const octomap::point3d & point)
+{
+	if(point.x() < minValues_[0])
+	{
+		minValues_[0] = point.x();
+	}
+	if(point.y() < minValues_[1])
+	{
+		minValues_[1] = point.y();
+	}
+	if(point.z() < minValues_[2])
+	{
+		minValues_[2] = point.z();
+	}
+	if(point.x() > maxValues_[0])
+	{
+		maxValues_[0] = point.x();
+	}
+	if(point.y() > maxValues_[1])
+	{
+		maxValues_[1] = point.y();
+	}
+	if(point.z() > maxValues_[2])
+	{
+		maxValues_[2] = point.z();
+	}
+}
+
+void OctoMap::HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
 {
 	int i;
 	float f, p, q, t;
@@ -475,7 +711,9 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 		unsigned int treeDepth,
 		std::vector<int> * obstacleIndices,
-		std::vector<int> * emptyIndices) const
+		std::vector<int> * emptyIndices,
+		std::vector<int> * groundIndices,
+		bool originalRefPoints) const
 {
 	UASSERT(treeDepth <= octree_->getTreeDepth());
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -490,22 +728,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	{
 		emptyIndices->resize(octree_->size());
 	}
+	if(groundIndices)
+	{
+		groundIndices->resize(octree_->size());
+	}

 	if(treeDepth == 0)
 	{
 		treeDepth = octree_->getTreeDepth();
 	}

-	double minX, minY, minZ, maxX, maxY, maxZ;
-	octree_->getMetricMin(minX, minY, minZ);
-	octree_->getMetricMax(maxX, maxY, maxZ);
+	double minZ = minValues_[2];
+	double maxZ = maxValues_[2];

+	bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
 	int oi=0;
 	int si=0;
+	int ei=0;
 	int gi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
+	float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
+	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
-		if(octree_->isNodeOccupied(*it) && (obstacleIndices || emptyIndices == 0))
+		if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || addAllPoints))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
@@ -522,25 +766,45 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 				(*cloud)[oi].g = g*255.0f;
 				(*cloud)[oi].b = b*255.0f;
 			}
-			(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
-			(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
-			(*cloud)[oi].z = pt.z();
+
+			if(originalRefPoints && it->getOccupancyType() > 0)
+			{
+				const octomap::point3d & p = it->getPointRef();
+				(*cloud)[oi].x = p.x();
+				(*cloud)[oi].y = p.y();
+				(*cloud)[oi].z = p.z();
+			}
+			else
+			{
+				(*cloud)[oi].x = pt.x()-halfCellSize;
+				(*cloud)[oi].y = pt.y()-halfCellSize;
+				(*cloud)[oi].z = pt.z();
+			}
+
 			if(obstacleIndices)
 			{
 				obstacleIndices->at(si++) = oi;
 			}
+
 			++oi;
 		}
-		else if(emptyIndices || obstacleIndices == 0)
+		else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || groundIndices != 0 || addAllPoints))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
-			(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
-			(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
+			(*cloud)[oi].x = pt.x()-halfCellSize;
+			(*cloud)[oi].y = pt.y()-halfCellSize;
 			(*cloud)[oi].z = pt.z();
-			if(emptyIndices)
+			if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
 			{
-				emptyIndices->at(gi++) = oi;
+				if(groundIndices)
+				{
+					groundIndices->at(gi++) = oi;
+				}
+			}
+			else if(emptyIndices)
+			{
+				emptyIndices->at(ei++) = oi;
 			}
 			++oi;
 		}
@@ -550,10 +814,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	if(obstacleIndices)
 	{
 		obstacleIndices->resize(si);
+		UDEBUG("obstacle=%d", si);
 	}
 	if(emptyIndices)
 	{
-		emptyIndices->resize(gi);
+		emptyIndices->resize(ei);
+		UDEBUG("empty=%d", ei);
+	}
+	if(groundIndices)
+	{
+		groundIndices->resize(gi);
+		UDEBUG("ground=%d", gi);
 	}

 	UDEBUG("");
@@ -569,6 +840,7 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	}

 	gridCellSize = octree_->getNodeSize(treeDepth);
+	float halfCellSize = gridCellSize/2.0f;

 	pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -577,16 +849,16 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	obstacles->resize(octree_->size());
 	int gi=0;
 	int oi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
+	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
 		octomap::point3d pt = octree_->keyToCoord(it.getKey());
 		if(octree_->isNodeOccupied(*it))
 		{
-			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
+			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
 		}
 		else
 		{
-			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
+			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
 		}
 	}
 	obstacles->resize(oi);
@@ -594,11 +866,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel

 	if(obstacles->size())
 	{
-		obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
+		obstacles = util3d::voxelize(obstacles, halfCellSize);
 	}
 	if(ground->size())
 	{
-		ground = util3d::voxelize(ground, gridCellSize/2.0f);
+		ground = util3d::voxelize(ground, halfCellSize);
 	}

 	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	if(removedParameters_.empty())
 	{
 		// removed parameters
+
+		// 0.16.0
+		removedParameters_.insert(std::make_pair("Grid/ProjRayTracing",           std::make_pair(true,  Parameters::kGridRayTracing())));
+
 		// 0.15.1
 		removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount",  std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("Reg/VarianceNormalized",        std::make_pair(false, "")));
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -2128,7 +2128,7 @@ bool Rtabmap::process(
 									}
 									else
 									{
-										UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
+										UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
 									}
 								}
 							}
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -476,12 +476,14 @@ void SensorData::setUserData(const cv::Mat & userData)
 void SensorData::setOccupancyGrid(
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint)
 {
-	UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
+	UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
 	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
-	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
+	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
+	   (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
 	{
 		UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
 		return;
@@ -491,9 +493,12 @@ void SensorData::setOccupancyGrid(
 	_groundCellsCompressed = cv::Mat();
 	_obstacleCellsRaw = cv::Mat();
 	_obstacleCellsCompressed = cv::Mat();
+	_emptyCellsRaw = cv::Mat();
+	_emptyCellsCompressed = cv::Mat();

 	CompressionThread ctGround(ground);
 	CompressionThread ctObstacles(obstacles);
+	CompressionThread ctEmpty(empty);

 	if(!ground.empty())
 	{
@@ -521,8 +526,22 @@ void SensorData::setOccupancyGrid(
 			_obstacleCellsCompressed = obstacles;
 		}
 	}
+	if(!empty.empty())
+	{
+		if(empty.type() == CV_32FC2 || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(5) || empty.type() == CV_32FC(6) || empty.type() == CV_32FC(7))
+		{
+			_emptyCellsRaw = empty;
+			ctEmpty.start();
+		}
+		else if(empty.type() == CV_8UC1)
+		{
+			UASSERT(empty.type() == CV_8UC1); // Bytes
+			_emptyCellsCompressed = empty;
+		}
+	}
 	ctGround.join();
 	ctObstacles.join();
+	ctEmpty.join();
 	if(!_groundCellsRaw.empty())
 	{
 		_groundCellsCompressed = ctGround.getCompressedData();
@@ -531,6 +550,10 @@ void SensorData::setOccupancyGrid(
 	{
 		_obstacleCellsCompressed = ctObstacles.getCompressedData();
 	}
+	if(!_emptyCellsRaw.empty())
+	{
+		_emptyCellsCompressed = ctEmpty.getCompressedData();
+	}

 	_cellSize = cellSize;
 	_viewPoint = viewPoint;
@@ -538,13 +561,14 @@ void SensorData::setOccupancyGrid(

 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
+	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF, tmpG;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
 				_laserScanCompressed.empty()?0:&tmpC,
 				_userDataCompressed.empty()?0:&tmpD,
 				_groundCellsCompressed.empty()?0:&tmpE,
-				_obstacleCellsCompressed.empty()?0:&tmpF);
+				_obstacleCellsCompressed.empty()?0:&tmpF,
+				_emptyCellsCompressed.empty()?0:&tmpG);
 }

 void SensorData::uncompressData(
@@ -553,15 +577,17 @@ void SensorData::uncompressData(
 		cv::Mat * laserScanRaw,
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
-		cv::Mat * obstacleCellsRaw)
+		cv::Mat * obstacleCellsRaw,
+		cv::Mat * emptyCellsRaw)
 {
-	UDEBUG("%d data(%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0);
+	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
 		userDataRaw == 0 &&
 		groundCellsRaw == 0 &&
-		obstacleCellsRaw == 0)
+		obstacleCellsRaw == 0 &&
+		emptyCellsRaw == 0)
 	{
 		return;
 	}
@@ -571,7 +597,8 @@ void SensorData::uncompressData(
 			laserScanRaw,
 			userDataRaw,
 			groundCellsRaw,
-			obstacleCellsRaw);
+			obstacleCellsRaw,
+			emptyCellsRaw);

 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
@@ -609,6 +636,10 @@ void SensorData::uncompressData(
 	{
 		_obstacleCellsRaw = *obstacleCellsRaw;
 	}
+	if(emptyCellsRaw && !emptyCellsRaw->empty() && _emptyCellsRaw.empty())
+	{
+		_emptyCellsRaw = *emptyCellsRaw;
+	}
 }

 void SensorData::uncompressDataConst(
@@ -617,7 +648,8 @@ void SensorData::uncompressDataConst(
 		cv::Mat * laserScanRaw,
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
-		cv::Mat * obstacleCellsRaw) const
+		cv::Mat * obstacleCellsRaw,
+		cv::Mat * emptyCellsRaw) const
 {
 	if(imageRaw)
 	{
@@ -643,12 +675,17 @@ void SensorData::uncompressDataConst(
 	{
 		*obstacleCellsRaw = _obstacleCellsRaw;
 	}
+	if(emptyCellsRaw)
+	{
+		*emptyCellsRaw = _emptyCellsRaw;
+	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()) ||
 		(userDataRaw && userDataRaw->empty()) ||
 		(groundCellsRaw && groundCellsRaw->empty()) ||
-		(obstacleCellsRaw && obstacleCellsRaw->empty()))
+		(obstacleCellsRaw && obstacleCellsRaw->empty()) ||
+		(emptyCellsRaw && emptyCellsRaw->empty()))
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
@@ -656,6 +693,7 @@ void SensorData::uncompressDataConst(
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
 		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
 		rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
+		rtabmap::CompressionThread ctEmptyCells(_emptyCellsCompressed, false);
 		if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
 		{
 			UASSERT(_imageCompressed.type() == CV_8UC1);
@@ -686,12 +724,18 @@ void SensorData::uncompressDataConst(
 			UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
 			ctObstacleCells.start();
 		}
+		if(emptyCellsRaw && emptyCellsRaw->empty() && !_emptyCellsCompressed.empty())
+		{
+			UASSERT(_emptyCellsCompressed.type() == CV_8UC1);
+			ctEmptyCells.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
 		ctUserData.join();
 		ctGroundCells.join();
 		ctObstacleCells.join();
+		ctEmptyCells.join();

 		if(imageRaw && imageRaw->empty())
 		{
@@ -763,6 +807,10 @@ void SensorData::uncompressDataConst(
 		{
 			*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
 		}
+		if(emptyCellsRaw && emptyCellsRaw->empty())
+		{
+			*emptyCellsRaw = ctEmptyCells.getUncompressedData();
+		}
 	}
 }

@@ -789,6 +837,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
 			_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
 			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
+			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
+			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
 			_keypoints.size() * sizeof(float) * 7 +
 			_keypoints3D.size() * sizeof(float)*3 +
 			_descriptors.total()*_descriptors.elemSize();
--- a/corelib/src/resources/DatabaseSchema.sql.in
+++ b/corelib/src/resources/DatabaseSchema.sql.in
@@ -39,6 +39,7 @@ CREATE TABLE Data (
 	
 	ground_cells BLOB,        -- compressed data (occupancy grid)
 	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
 	cell_size FLOAT,
 	view_point_x FLOAT,
 	view_point_y FLOAT,
@@ -114,6 +115,9 @@ CREATE TABLE Admin (
 	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
 	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
 	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 

 	time_enter DATE
 );
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -1358,27 +1358,60 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co

 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform)
 {
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(4));
+	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform);
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform)
+{
+	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
-	for(unsigned int i=0; i<cloud.size(); ++i)
+	if(indices.get())
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
-		if(!nullTransform)
+		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
+		for(unsigned int i=0; i<indices->size(); ++i)
 		{
-			pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
-			ptr[0] = pt.x;
-			ptr[1] = pt.y;
-			ptr[2] = pt.z;
+			float * ptr = laserScan.ptr<float>(0, i);
+			int index = indices->at(i);
+			if(!nullTransform)
+			{
+				pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
+				ptr[0] = pt.x;
+				ptr[1] = pt.y;
+				ptr[2] = pt.z;
+			}
+			else
+			{
+				ptr[0] = cloud.at(index).x;
+				ptr[1] = cloud.at(index).y;
+				ptr[2] = cloud.at(index).z;
+			}
+			int * ptrInt = (int*)ptr;
+			ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
 		}
-		else
+	}
+	else
+	{
+		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
+		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			ptr[0] = cloud.at(i).x;
-			ptr[1] = cloud.at(i).y;
-			ptr[2] = cloud.at(i).z;
+			float * ptr = laserScan.ptr<float>(0, i);
+			if(!nullTransform)
+			{
+				pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
+				ptr[0] = pt.x;
+				ptr[1] = pt.y;
+				ptr[2] = pt.z;
+			}
+			else
+			{
+				ptr[0] = cloud.at(i).x;
+				ptr[1] = cloud.at(i).y;
+				ptr[2] = cloud.at(i).z;
+			}
+			int * ptrInt = (int*)ptr;
+			ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
 		}
-		int * ptrInt = (int*)ptr;
-		ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
 	}
 	return laserScan;
 }
--- a/corelib/src/util3d_mapping.cpp
+++ b/corelib/src/util3d_mapping.cpp
@@ -48,8 +48,8 @@ namespace util3d

 void occupancy2DFromLaserScan(
 		const cv::Mat & scan,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
@@ -59,8 +59,8 @@ void occupancy2DFromLaserScan(
 			scan,
 			cv::Mat(),
 			viewpoint,
-			ground,
-			obstacles,
+			empty,
+			occupied,
 			cellSize,
 			unknownSpaceFilled,
 			scanMaxRange);
@@ -69,21 +69,21 @@ void occupancy2DFromLaserScan(
 void occupancy2DFromLaserScan(
 		const cv::Mat & scan,
 		const cv::Point3f & viewpoint,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
 {
-	occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, ground, obstacles, cellSize, unknownSpaceFilled, scanMaxRange);
+	occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, empty, occupied, cellSize, unknownSpaceFilled, scanMaxRange);
 }

 void occupancy2DFromLaserScan(
 		const cv::Mat & scanHit,
 		const cv::Mat & scanNoHit,
 		const cv::Point3f & viewpoint,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
@@ -105,27 +105,27 @@ void occupancy2DFromLaserScan(
 	float xMin, yMin;
 	cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);

-	// find ground cells
-	std::list<int> groundIndices;
+	// find empty cells
+	std::list<int> emptyIndices;
 	for(unsigned int i=0; i< map8S.total(); ++i)
 	{
 		if(map8S.data[i] == 0)
 		{
-			groundIndices.push_back(i);
+			emptyIndices.push_back(i);
 		}
 	}

 	// Convert to position matrices, get points to each center of the cells
-	ground = cv::Mat();
-	if(groundIndices.size())
+	empty = cv::Mat();
+	if(emptyIndices.size())
 	{
-		ground = cv::Mat(1, (int)groundIndices.size(), CV_32FC2);
+		empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
 		int i=0;
-		for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
+		for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
 		{
 			int y = *iter / map8S.cols;
 			int x = *iter - y*map8S.cols;
-			cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
+			cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
 			ptr[i][0] = (float(x))*cellSize + xMin;
 			ptr[i][1] = (float(y))*cellSize + yMin;
 			++i;
@@ -133,7 +133,7 @@ void occupancy2DFromLaserScan(
 	}

 	// copy directly obstacles precise positions
-	obstacles = scanHit.clone();
+	occupied = scanHit.clone();
 }

 /**