GridMap integration (#1180)

* GridMap integration

* Removed GridGlobal/FullUpdate parameter. Bump version 0.21.3. Mvoed specialized global map classes under global_map sub dir. Renamed Map -> GlobalMap.

* UI: Added elevation map visualization

* Added LocalGridCache class to share cache between global maps

* Fixed OctoMap nans. DbViewer: Added frontiers visualization.

* convenient functions for ros

* Small fix

* fixed build without GridMap

* CI disabled fail-fast

* CI updated checkout action to v4

corelib/src/CMakeLists.txt

@@ -107,7 +107,11 @@ SET(SRC_FILES
     stereo/StereoBM.cpp
     stereo/StereoSGBM.cpp
     
-    OccupancyGrid.cpp
+    GlobalMap.cpp
+    LocalGridMaker.cpp
+    LocalGrid.cpp
+    global_map/OccupancyGrid.cpp
+    global_map/CloudMap.cpp
     
     MarkerDetector.cpp
     
@@ -590,10 +594,32 @@ IF(octomap_FOUND)
     ENDIF()
 	SET(SRC_FILES
     	${SRC_FILES}
-		OctoMap.cpp
+		global_map/OctoMap.cpp
 	)
 ENDIF(octomap_FOUND)
 
+IF(grid_map_core_FOUND)
+    IF(TARGET grid_map_core)
+        SET(PUBLIC_LIBRARIES
+            ${PUBLIC_LIBRARIES}
+            grid_map_core
+        )
+    ELSE()
+        SET(PUBLIC_INCLUDE_DIRS
+            ${PUBLIC_INCLUDE_DIRS}
+            ${grid_map_core_INCLUDE_DIRS}
+        )
+        SET(PUBLIC_LIBRARIES
+            ${PUBLIC_LIBRARIES}
+            ${grid_map_core_LIBRARIES}
+        )
+    ENDIF()
+	SET(SRC_FILES
+    	${SRC_FILES}
+		global_map/GridMap.cpp
+	)
+ENDIF(grid_map_core_FOUND)
+
 IF(AliceVision_FOUND)
   SET(LIBRARIES
     ${LIBRARIES}

corelib/src/GlobalMap.cpp

@@ -0,0 +1,169 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/GlobalMap.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+
+namespace rtabmap {
+
+GlobalMap::GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters) :
+	cellSize_(Parameters::defaultGridCellSize()),
+	updateError_(Parameters::defaultGridGlobalUpdateError()),
+	occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
+	logOddsHit_(logodds(Parameters::defaultGridGlobalProbHit())),
+	logOddsMiss_(logodds(Parameters::defaultGridGlobalProbMiss())),
+	logOddsClampingMin_(logodds(Parameters::defaultGridGlobalProbClampingMin())),
+	logOddsClampingMax_(logodds(Parameters::defaultGridGlobalProbClampingMax())),
+	cache_(cache)
+{
+	UASSERT(cache_);
+
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
+	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
+	UASSERT(cellSize_>0.0f);
+
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
+
+	UDEBUG("cellSize_           =%f", cellSize_);
+	UDEBUG("updateError_        =%f", updateError_);
+
+	// Probabilistic parameters
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), logOddsHit_))
+	{
+		logOddsHit_ = logodds(logOddsHit_);
+		UASSERT_MSG(logOddsHit_ >= 0.0f, uFormat("probHit_=%f",logOddsHit_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), logOddsMiss_))
+	{
+		logOddsMiss_ = logodds(logOddsMiss_);
+		UASSERT_MSG(logOddsMiss_ <= 0.0f, uFormat("probMiss_=%f",logOddsMiss_).c_str());
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), logOddsClampingMin_))
+	{
+		logOddsClampingMin_ = logodds(logOddsClampingMin_);
+	}
+	if(Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), logOddsClampingMax_))
+	{
+		logOddsClampingMax_ = logodds(logOddsClampingMax_);
+	}
+	UASSERT(logOddsClampingMax_ > logOddsClampingMin_);
+}
+
+GlobalMap::~GlobalMap()
+{
+	clear();
+}
+
+void GlobalMap::clear()
+{
+	UDEBUG("Clearing");
+	addedNodes_.clear();
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+}
+
+unsigned long GlobalMap::getMemoryUsed() const
+{
+	unsigned long memoryUsage = 0;
+
+	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::map<int, Transform>::iterator)) + sizeof(std::map<int, Transform>);
+
+	return memoryUsage;
+}
+
+bool GlobalMap::update(const std::map<int, Transform> & poses)
+{
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	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
+	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);
+		if(jter != poses.end())
+		{
+			graphChanged = false;
+			UASSERT(!iter->second.isNull() && !jter->second.isNull());
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
+			{
+				graphOptimized = true;
+			}
+		}
+		else
+		{
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
+		}
+	}
+
+	if(graphOptimized || graphChanged)
+	{
+		// clear all but keep cache
+		clear();
+	}
+
+	std::list<std::pair<int, Transform> > orderedPoses;
+
+	// add old poses that were not in the current map (they were just retrieved from LTM)
+	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
+	{
+		if(!isNodeAssembled(iter->first))
+		{
+			UDEBUG("Pose %d not found in current added poses, it will be added to map", iter->first);
+			orderedPoses.push_back(*iter);
+		}
+	}
+
+	// insert zero after
+	if(poses.find(0) != poses.end())
+	{
+		orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
+	}
+
+	if(!orderedPoses.empty())
+	{
+		assemble(orderedPoses);
+	}
+
+	return !orderedPoses.empty();
+}
+
+void GlobalMap::addAssembledNode(int id, const Transform & pose)
+{
+	if(id > 0)
+	{
+		uInsert(addedNodes_, std::make_pair(id, pose));
+	}
+}
+
+
+} // namespace rtabmap

corelib/src/LocalGrid.cpp

@@ -0,0 +1,126 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/GlobalMap.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+
+namespace rtabmap {
+
+LocalGrid::LocalGrid(const cv::Mat & groundIn,
+			 const cv::Mat & obstaclesIn,
+			 const cv::Mat & emptyIn,
+			 float cellSizeIn,
+			 const cv::Point3f & viewPointIn) :
+		groundCells(groundIn),
+		obstacleCells(obstaclesIn),
+		emptyCells(emptyIn),
+		cellSize(cellSizeIn),
+		viewPoint(viewPointIn)
+{
+	UASSERT(cellSize > 0.0f);
+}
+
+bool LocalGrid::is3D() const
+{
+	return (groundCells.empty() || groundCells.type() == CV_32FC3 || groundCells.type() == CV_32FC(4) || groundCells.type() == CV_32FC(6)) &&
+		   (obstacleCells.empty() || obstacleCells.type() == CV_32FC3 || obstacleCells.type() == CV_32FC(4) || obstacleCells.type() == CV_32FC(6)) &&
+		   (emptyCells.empty() || emptyCells.type() == CV_32FC3 || emptyCells.type() == CV_32FC(4) || emptyCells.type() == CV_32FC(6));
+}
+
+void LocalGridCache::add(int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		const cv::Mat & empty,
+		float cellSize,
+		const cv::Point3f & viewPoint)
+{
+	add(nodeId, LocalGrid(ground, obstacles, empty, cellSize, viewPoint));
+}
+
+void LocalGridCache::add(int nodeId, const LocalGrid & localGrid)
+{
+	UDEBUG("nodeId=%d (ground=%d/%d obstacles=%d/%d empty=%d/%d)",
+			nodeId, localGrid.groundCells.cols,  localGrid.groundCells.channels(),  localGrid.obstacleCells.cols,  localGrid.obstacleCells.channels(), localGrid.emptyCells.cols,  localGrid.emptyCells.channels());
+	if(nodeId < 0)
+	{
+		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
+		return;
+	}
+	uInsert(localGrids_, std::make_pair(nodeId==0?-1:nodeId, localGrid));
+}
+
+bool LocalGridCache::shareTo(int nodeId, LocalGridCache & anotherCache) const
+{
+	if(uContains(localGrids_, nodeId) && !uContains(anotherCache.localGrids(), nodeId))
+	{
+		const LocalGrid & localGrid = localGrids_.at(nodeId);
+		anotherCache.add(nodeId, localGrid.groundCells, localGrid.obstacleCells, localGrid.emptyCells, localGrid.cellSize, localGrid.viewPoint);
+		return true;
+	}
+	return false;
+}
+
+unsigned long LocalGridCache::getMemoryUsed() const
+{
+	unsigned long memoryUsage = 0;
+	memoryUsage += localGrids_.size()*(sizeof(int) + sizeof(LocalGrid) + sizeof(std::map<int, LocalGrid>::iterator)) + sizeof(std::map<int, LocalGrid>);
+	for(std::map<int, LocalGrid>::const_iterator iter=localGrids_.begin(); iter!=localGrids_.end(); ++iter)
+	{
+		memoryUsage += iter->second.groundCells.total() * iter->second.groundCells.elemSize();
+		memoryUsage += iter->second.obstacleCells.total() * iter->second.obstacleCells.elemSize();
+		memoryUsage += iter->second.emptyCells.total() * iter->second.emptyCells.elemSize();
+		memoryUsage += sizeof(int);
+		memoryUsage += sizeof(cv::Point3f);
+	}
+	return memoryUsage;
+}
+
+void LocalGridCache::clear(bool temporaryOnly)
+{
+	if(temporaryOnly)
+	{
+		//clear only negative ids
+		for(std::map<int, LocalGrid>::iterator iter=localGrids_.begin(); iter!=localGrids_.end();)
+		{
+			if(iter->first < 0)
+			{
+				localGrids_.erase(iter++);
+			}
+			else
+			{
+				break;
+			}
+		}
+	}
+	else
+	{
+		localGrids_.clear();
+	}
+}
+
+} // namespace rtabmap

corelib/src/LocalGridMaker.cpp

@@ -0,0 +1,587 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/LocalGridMaker.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/global_map/OctoMap.h>
+#endif
+
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+LocalGridMaker::LocalGridMaker(const ParametersMap & parameters) :
+	parameters_(parameters),
+	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
+	rangeMax_(Parameters::defaultGridRangeMax()),
+	rangeMin_(Parameters::defaultGridRangeMin()),
+	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
+	footprintLength_(Parameters::defaultGridFootprintLength()),
+	footprintWidth_(Parameters::defaultGridFootprintWidth()),
+	footprintHeight_(Parameters::defaultGridFootprintHeight()),
+	scanDecimation_(Parameters::defaultGridScanDecimation()),
+	cellSize_(Parameters::defaultGridCellSize()),
+	preVoxelFiltering_(Parameters::defaultGridPreVoxelFiltering()),
+	occupancySensor_(Parameters::defaultGridSensor()),
+	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
+	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
+	normalKSearch_(Parameters::defaultGridNormalK()),
+	groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
+	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
+	clusterRadius_(Parameters::defaultGridClusterRadius()),
+	minClusterSize_(Parameters::defaultGridMinClusterSize()),
+	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
+	minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
+	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
+	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
+	grid3D_(Parameters::defaultGrid3D()),
+	groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
+	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
+	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
+	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
+	rayTracing_(Parameters::defaultGridRayTracing())
+{
+	this->parseParameters(parameters);
+}
+
+LocalGridMaker::~LocalGridMaker()
+{
+}
+
+void LocalGridMaker::parseParameters(const ParametersMap & parameters)
+{
+	uInsert(parameters_, parameters);
+
+	Parameters::parse(parameters, Parameters::kGridSensor(), occupancySensor_);
+	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
+	if(cloudDecimation_ == 0)
+	{
+		cloudDecimation_ = 1;
+	}
+	Parameters::parse(parameters, Parameters::kGridRangeMin(), rangeMin_);
+	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
+	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
+	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
+	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
+	Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
+	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
+	UASSERT(cellSize_>0.0f);
+
+	Parameters::parse(parameters, Parameters::kGridPreVoxelFiltering(), preVoxelFiltering_);
+	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
+	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
+	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
+	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
+	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
+	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
+	{
+		maxGroundAngle_ *= M_PI/180.0f;
+	}
+	Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
+	UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
+	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
+	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
+	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
+	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
+	Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
+	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
+	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
+
+	// convert ROI from string to vector
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
+	{
+		std::list<std::string> strValues = uSplit(iter->second, ' ');
+		if(strValues.size() != 4)
+		{
+			ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+		}
+		else
+		{
+			std::vector<float> tmpValues(4);
+			unsigned int i=0;
+			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+			{
+				tmpValues[i] = uStr2Float(*jter);
+				++i;
+			}
+
+			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+			{
+				roiRatios_ = tmpValues;
+			}
+			else
+			{
+				ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+			}
+		}
+	}
+
+	if(maxGroundHeight_ == 0.0f && !normalsSegmentation_)
+	{
+		UWARN("\"%s\" should be not equal to 0 if not using normals "
+				"segmentation approach. Setting it to cell size (%f).",
+				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
+		maxGroundHeight_ = cellSize_;
+	}
+	if(maxGroundHeight_ != 0.0f &&
+		maxObstacleHeight_ != 0.0f &&
+		maxObstacleHeight_ < maxGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str());
+		maxObstacleHeight_ = 0;
+	}
+	if(maxGroundHeight_ != 0.0f &&
+		minGroundHeight_ != 0.0f &&
+		maxGroundHeight_ < minGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMinGroundHeight().c_str(),
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMinGroundHeight().c_str());
+		minGroundHeight_ = 0;
+	}
+}
+
+void LocalGridMaker::createLocalMap(
+		const Signature & node,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPoint)
+{
+	UDEBUG("scan format=%s, occupancySensor_=%d normalsSegmentation_=%d grid3D_=%d",
+			node.sensorData().laserScanRaw().isEmpty()?"NA":node.sensorData().laserScanRaw().formatName().c_str(), occupancySensor_, normalsSegmentation_?1:0, grid3D_?1:0);
+
+	if((node.sensorData().laserScanRaw().is2d()) && occupancySensor_ == 0)
+	{
+		UDEBUG("2D laser scan");
+		//2D
+		viewPoint = cv::Point3f(
+				node.sensorData().laserScanRaw().localTransform().x(),
+				node.sensorData().laserScanRaw().localTransform().y(),
+				node.sensorData().laserScanRaw().localTransform().z());
+
+		LaserScan scan = node.sensorData().laserScanRaw();
+		if(rangeMin_ > 0.0f)
+		{
+			scan = util3d::rangeFiltering(scan, rangeMin_, 0.0f);
+		}
+
+		float maxRange = rangeMax_;
+		if(rangeMax_>0.0f && node.sensorData().laserScanRaw().rangeMax()>0.0f)
+		{
+			maxRange = rangeMax_ < node.sensorData().laserScanRaw().rangeMax()?rangeMax_:node.sensorData().laserScanRaw().rangeMax();
+		}
+		else if(scan2dUnknownSpaceFilled_ && node.sensorData().laserScanRaw().rangeMax()>0.0f)
+		{
+			maxRange = node.sensorData().laserScanRaw().rangeMax();
+		}
+		util3d::occupancy2DFromLaserScan(
+				util3d::transformLaserScan(scan, node.sensorData().laserScanRaw().localTransform()).data(),
+				cv::Mat(),
+				viewPoint,
+				emptyCells,
+				obstacleCells,
+				cellSize_,
+				scan2dUnknownSpaceFilled_,
+				maxRange);
+
+		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
+	}
+	else
+	{
+		// 3D
+		if(occupancySensor_ == 0 || occupancySensor_ == 2)
+		{
+			if(!node.sensorData().laserScanRaw().isEmpty())
+			{
+				UDEBUG("3D laser scan");
+				const Transform & t = node.sensorData().laserScanRaw().localTransform();
+				LaserScan scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
+#ifdef RTABMAP_OCTOMAP
+				// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
+				float maxRange = rayTracing_?0.0f:rangeMax_;
+#else
+				// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+				float maxRange = !grid3D_ && rayTracing_?0.0f:rangeMax_;
+#endif
+				if(rangeMin_ > 0.0f || maxRange > 0.0f)
+				{
+					scan = util3d::rangeFiltering(scan, rangeMin_, maxRange);
+				}
+
+				// update viewpoint
+				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+
+				UDEBUG("scan format=%d", scan.format());
+
+				bool normalSegmentationTmp = normalsSegmentation_;
+				float minGroundHeightTmp = minGroundHeight_;
+				float maxGroundHeightTmp = maxGroundHeight_;
+				if(scan.is2d())
+				{
+					// if 2D, assume the whole scan is obstacle
+					normalsSegmentation_ = false;
+					minGroundHeight_ = std::numeric_limits<int>::min();
+					maxGroundHeight_ = std::numeric_limits<int>::min()+100;
+				}
+
+				createLocalMap(scan, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+				if(scan.is2d())
+				{
+					// restore
+					normalsSegmentation_ = normalSegmentationTmp;
+					minGroundHeight_ = minGroundHeightTmp;
+					maxGroundHeight_ = maxGroundHeightTmp;
+				}
+			}
+			else
+			{
+				UWARN("Cannot create local map from scan: scan is empty (node=%d, %s=%d).", node.id(), Parameters::kGridSensor().c_str(), occupancySensor_);
+			}
+		}
+
+		if(occupancySensor_ >= 1)
+		{
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+			UDEBUG("Depth image : decimation=%d max=%f min=%f",
+					cloudDecimation_,
+					rangeMax_,
+					rangeMin_);
+			cloud = util3d::cloudRGBFromSensorData(
+					node.sensorData(),
+					cloudDecimation_,
+#ifdef RTABMAP_OCTOMAP
+					// If ray tracing enabled, clipping will be done in OctoMap or in occupancy2DFromLaserScan()
+					rayTracing_?0.0f:rangeMax_,
+#else
+					// If ray tracing enabled, clipping will be done in occupancy2DFromLaserScan()
+					!grid3D_&&rayTracing_?0.0f:rangeMax_,
+#endif
+					rangeMin_,
+					indices.get(),
+					parameters_,
+					roiRatios_);
+
+			// update viewpoint
+			viewPoint = cv::Point3f(0,0,0);
+			if(node.sensorData().cameraModels().size())
+			{
+				// average of all local transforms
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().cameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
+			}
+			else
+			{
+				// average of all local transforms
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().stereoCameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
+			}
+
+			cv::Mat scanGroundCells;
+			cv::Mat scanObstacleCells;
+			cv::Mat scanEmptyCells;
+			if(occupancySensor_ == 2)
+			{
+				// backup
+				scanGroundCells = groundCells;
+				scanObstacleCells = obstacleCells;
+				scanEmptyCells = emptyCells;
+				groundCells = cv::Mat();
+				obstacleCells = cv::Mat();
+				emptyCells = cv::Mat();
+			}
+
+			createLocalMap(LaserScan(util3d::laserScanFromPointCloud(*cloud, indices), 0, 0.0f), node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+
+			if(occupancySensor_ == 2)
+			{
+				if(grid3D_)
+				{
+					// We should convert scans to 4 channels (XYZRGB) to be compatible
+					scanGroundCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanGroundCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanObstacleCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanObstacleCells), Transform::getIdentity(), 255, 255, 255)).data();
+					scanEmptyCells = util3d::laserScanFromPointCloud(*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(scanEmptyCells), Transform::getIdentity(), 255, 255, 255)).data();
+				}
+
+				UDEBUG("groundCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", groundCells.cols, groundCells.channels(), scanGroundCells.cols, scanGroundCells.channels());
+				UDEBUG("obstacleCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", obstacleCells.cols, obstacleCells.channels(), scanObstacleCells.cols, scanObstacleCells.channels());
+				UDEBUG("emptyCells, depth: size=%d channels=%d vs scan: size=%d channels=%d", emptyCells.cols, emptyCells.channels(), scanEmptyCells.cols, scanEmptyCells.channels());
+
+				if(!groundCells.empty() && !scanGroundCells.empty())
+					cv::hconcat(groundCells, scanGroundCells, groundCells);
+				else if(!scanGroundCells.empty())
+					groundCells = scanGroundCells;
+
+				if(!obstacleCells.empty() && !scanObstacleCells.empty())
+					cv::hconcat(obstacleCells, scanObstacleCells, obstacleCells);
+				else if(!scanObstacleCells.empty())
+					obstacleCells = scanObstacleCells;
+
+				if(!emptyCells.empty() && !scanEmptyCells.empty())
+					cv::hconcat(emptyCells, scanEmptyCells, emptyCells);
+				else if(!scanEmptyCells.empty())
+					emptyCells = scanEmptyCells;
+			}
+		}
+	}
+}
+
+void LocalGridMaker::createLocalMap(
+		const LaserScan & scan,
+		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(scan.size())
+	{
+		pcl::IndicesPtr groundIndices(new std::vector<int>);
+		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+		cv::Mat groundCloud;
+		cv::Mat obstaclesCloud;
+
+		if(scan.hasRGB() && scan.hasNormals())
+		{
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGBNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGBNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else if(scan.hasRGB())
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZRGB>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else if(scan.hasNormals())
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloudSegmented = segmentCloud<pcl::PointNormal>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointNormal>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSegmented = segmentCloud<pcl::PointXYZ>(cloud, pcl::IndicesPtr(new std::vector<int>), pose, viewPointInOut, groundIndices, obstaclesIndices);
+			UDEBUG("groundIndices=%d, obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+			if(grid3D_)
+			{
+				groundCloud = util3d::laserScanFromPointCloud(*cloudSegmented, groundIndices).data();
+				obstaclesCloud = util3d::laserScanFromPointCloud(*cloudSegmented, obstaclesIndices).data();
+			}
+			else
+			{
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(cloudSegmented, groundIndices, obstaclesIndices, groundCells, obstacleCells, cellSize_);
+			}
+		}
+
+		if(grid3D_ && (!obstaclesCloud.empty() || !groundCloud.empty()))
+		{
+			UDEBUG("ground=%d obstacles=%d", groundCloud.cols, obstaclesCloud.cols);
+			if(groundIsObstacle_ && !groundCloud.empty())
+			{
+				if(obstaclesCloud.empty())
+				{
+					obstaclesCloud = groundCloud;
+					groundCloud = cv::Mat();
+				}
+				else
+				{
+					UASSERT(obstaclesCloud.type() == groundCloud.type());
+					cv::Mat merged(1,obstaclesCloud.cols+groundCloud.cols, obstaclesCloud.type());
+					obstaclesCloud.copyTo(merged(cv::Range::all(), cv::Range(0, obstaclesCloud.cols)));
+					groundCloud.copyTo(merged(cv::Range::all(), cv::Range(obstaclesCloud.cols, obstaclesCloud.cols+groundCloud.cols)));
+				}
+			}
+
+			// 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(rayTracing_)
+			{
+#ifdef RTABMAP_OCTOMAP
+				if(!groundCloud.empty() || !obstaclesCloud.empty())
+				{
+					//create local octomap
+					ParametersMap params;
+					params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_)));
+					params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(rangeMax_)));
+					params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_)));
+					LocalGridCache cache;
+					OctoMap octomap(&cache, params);
+					cache.add(1, groundCloud, obstaclesCloud, cv::Mat(), cellSize_, cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
+					std::map<int, Transform> poses;
+					poses.insert(std::make_pair(1, Transform::getIdentity()));
+					octomap.update(poses);
+
+					pcl::IndicesPtr groundIndices(new std::vector<int>);
+					pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+					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());
+					if(scan.hasRGB())
+					{
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv).data();
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv).data();
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv).data();
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithRayTracing2(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*cloudWithRayTracing, *cloudWithRayTracing2);
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, groundIndices, tinv).data();
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, obstaclesIndices, tinv).data();
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing2, emptyIndices, tinv).data();
+					}
+				}
+			}
+			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
+			{
+				groundCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(groundCloud), tinv).data();
+				obstacleCells = util3d::transformLaserScan(LaserScan::backwardCompatibility(obstaclesCloud), tinv).data();
+			}
+
+		}
+		else if(!grid3D_ && rayTracing_ && (!obstacleCells.empty() || !groundCells.empty()))
+		{
+			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
+					rangeMax_);
+		}
+	}
+	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
+}
+
+} // namespace rtabmap

corelib/src/Memory.cpp

@@ -60,9 +60,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/optimizer/OptimizerG2O.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
-#include <rtabmap/core/OccupancyGrid.h>
 #include <rtabmap/core/MarkerDetector.h>
 #include <opencv2/imgproc/types_c.h>
+#include <rtabmap/core/LocalGridMaker.h>
 
 namespace rtabmap {
 
@@ -153,7 +153,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	}
 	_registrationIcpMulti = new RegistrationIcp(paramsMulti);
 
-	_occupancy = new OccupancyGrid(parameters);
+	_localMapMaker = new LocalGridMaker(parameters);
 	_markerDetector = new MarkerDetector(parameters);
 	this->parseParameters(parameters);
 }
@@ -545,7 +545,7 @@ Memory::~Memory()
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
 	delete _registrationVis;
-	delete _occupancy;
+	delete _localMapMaker;
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -749,9 +749,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 	}
 
-	if(_occupancy)
+	if(_localMapMaker)
 	{
-		_occupancy->parseParameters(params);
+		_localMapMaker->parseParameters(params);
 	}
 
 	if(_markerDetector)
@@ -3706,7 +3706,7 @@ unsigned long Memory::getMemoryUsed() const
 	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(ParametersMap::iterator)) + sizeof(ParametersMap);
 	memoryUsage += sizeof(Registration);
 	memoryUsage += sizeof(RegistrationIcp);
-	memoryUsage += _occupancy->getMemoryUsed();
+	memoryUsage += sizeof(LocalGridMaker);
 	memoryUsage += sizeof(MarkerDetector);
 	memoryUsage += sizeof(DBDriver);
 
@@ -5842,14 +5842,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// Occupancy grid map stuff
 	if(_createOccupancyGrid && !isIntermediateNode)
 	{
-		if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
-			(!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty()))
+		if( (_localMapMaker->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
+			(!_localMapMaker->isGridFromDepth() && !data.laserScanRaw().empty()))
 		{
 			cv::Mat ground, obstacles, empty;
 			float cellSize = 0.0f;
 			cv::Point3f viewPoint(0,0,0);
-			_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
-			cellSize = _occupancy->getCellSize();
+			_localMapMaker->createLocalMap(*s, ground, obstacles, empty, viewPoint);
+			cellSize = _localMapMaker->getCellSize();
 			s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
 
 			t = timer.ticks();

corelib/src/Parameters.cpp

@@ -214,14 +214,14 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }
 
-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn, bool remove)
 {
 	ParametersMap output;
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		UASSERT(uSplit(iter->first, '/').size()  == 2);
 		std::string group = uSplit(iter->first, '/').front();
-		bool sameGroup = group.compare(group) == 0;
+		bool sameGroup = group.compare(groupIn) == 0;
 		if((!remove && sameGroup) || (remove && !sameGroup))
 		{
 			output.insert(*iter);
@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.21.3
+		removedParameters_.insert(std::make_pair("GridGlobal/FullUpdate",    std::make_pair(false, "")));
+
 		// 0.20.15
 		removedParameters_.insert(std::make_pair("Grid/FromDepth",           std::make_pair(true, Parameters::kGridSensor())));
 
@@ -301,7 +304,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy",            std::make_pair(true,  Parameters::kVhEpEnabled())));
 
 		// 0.12.5
-		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(true,  Parameters::kGridGlobalFullUpdate())));
+		removedParameters_.insert(std::make_pair("Grid/FullUpdate",               std::make_pair(false,  "")));
 
 		// 0.12.1
 		removedParameters_.insert(std::make_pair("Grid/3DGroundIsObstacle",       std::make_pair(true,  Parameters::kGridGroundIsObstacle())));
@@ -812,11 +815,17 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
-				str = "With octomap:";
+				str = "With OctoMap:";
 #ifdef RTABMAP_OCTOMAP
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With GridMap:";
+#ifdef RTABMAP_GRIDMAP
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With cpu-tsdf:";
 #ifdef RTABMAP_CPUTSDF

corelib/src/global_map/CloudMap.cpp

@@ -0,0 +1,153 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/global_map/CloudMap.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+CloudMap::CloudMap(const LocalGridCache * cache, const ParametersMap & parameters) :
+	GlobalMap(cache, parameters),
+	assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZ>)
+{
+}
+
+void CloudMap::clear()
+{
+	assembledGround_->clear();
+	assembledObstacles_->clear();
+	assembledEmptyCells_->clear();
+	GlobalMap::clear();
+}
+
+void CloudMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
+{
+	UTimer timer;
+
+	bool assembledGroundUpdated = false;
+	bool assembledObstaclesUpdated = false;
+	bool assembledEmptyCellsUpdated = false;
+
+	if(!cache().empty())
+	{
+		UDEBUG("Updating from cache");
+		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
+		{
+			if(uContains(cache(), iter->first))
+			{
+				const LocalGrid & localGrid = cache().at(iter->first);
+
+				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
+
+				addAssembledNode(iter->first, iter->second);
+
+				//ground
+				if(localGrid.groundCells.cols)
+				{
+					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
+					}
+
+					*assembledGround_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.groundCells), iter->second, 0, 255, 0);
+					assembledGroundUpdated = true;
+				}
+
+				//empty
+				if(localGrid.emptyCells.cols)
+				{
+					if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols);
+					}
+
+					*assembledEmptyCells_ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localGrid.emptyCells), iter->second);
+					assembledEmptyCellsUpdated = true;
+				}
+
+				//obstacles
+				if(localGrid.obstacleCells.cols)
+				{
+					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
+					}
+
+					*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(localGrid.obstacleCells), iter->second, 255, 0, 0);
+					assembledObstaclesUpdated = true;
+				}
+			}
+		}
+	}
+
+
+	if(assembledGroundUpdated && assembledGround_->size() > 1)
+	{
+		assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
+	}
+	if(assembledObstaclesUpdated && assembledGround_->size() > 1)
+	{
+		assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
+	}
+	if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
+	{
+		assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
+	}
+
+	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
+}
+
+unsigned long CloudMap::getMemoryUsed() const
+{
+	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
+
+	if(assembledGround_.get())
+	{
+		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledObstacles_.get())
+	{
+		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
+	}
+	if(assembledEmptyCells_.get())
+	{
+		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZ);
+	}
+	return memoryUsage;
+}
+
+}

corelib/src/global_map/GridMap.cpp

@@ -0,0 +1,485 @@
+/*
+Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/global_map/GridMap.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <list>
+
+#include <opencv2/photo.hpp>
+#include <grid_map_core/iterators/GridMapIterator.hpp>
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+GridMap::GridMap(const LocalGridCache * cache, const ParametersMap & parameters) :
+	GlobalMap(cache, parameters),
+	minMapSize_(Parameters::defaultGridGlobalMinSize())
+{
+	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
+}
+
+void GridMap::clear()
+{
+	gridMap_ = grid_map::GridMap();
+	GlobalMap::clear();
+}
+
+cv::Mat GridMap::createHeightMap(float & xMin, float & yMin, float & cellSize) const
+{
+	return toImage("elevation", xMin, yMin, cellSize);
+}
+
+cv::Mat GridMap::createColorMap(float & xMin, float & yMin, float & cellSize) const
+{
+	return toImage("colors", xMin, yMin, cellSize);
+}
+
+cv::Mat GridMap::toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const
+{
+	if( gridMap_.hasBasicLayers())
+	{
+		const grid_map::Matrix& data = gridMap_[layer];
+
+		cv::Mat image;
+		if(layer.compare("elevation") == 0)
+		{
+			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_32FC1);
+			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
+				const grid_map::Index index(*iterator);
+				const float& value = data(index(0), index(1));
+				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
+				if (std::isfinite(value))
+				{
+					image.at<float>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0)) = value;
+				}
+			}
+		}
+		else if(layer.compare("colors") == 0)
+		{
+			image = cv::Mat::zeros(gridMap_.getSize()(1), gridMap_.getSize()(0), CV_8UC3);
+			for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator) {
+				const grid_map::Index index(*iterator);
+				const float& value = data(index(0), index(1));
+				const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
+				if (std::isfinite(value))
+				{
+					const int * ptr = (const int *)&value;
+					cv::Vec3b & color = image.at<cv::Vec3b>(image.rows-1-imageIndex(1), image.cols-1-imageIndex(0));
+					color[0] = (unsigned char)(*ptr & 0xFF); // B
+					color[1] = (unsigned char)((*ptr >> 8) & 0xFF); // G
+					color[2] = (unsigned char)((*ptr >> 16) & 0xFF); // R
+				}
+			}
+		}
+		else
+		{
+			UFATAL("Unknown layer \"%s\"", layer.c_str());
+		}
+
+		xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
+		yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
+		cellSize = gridMap_.getResolution();
+
+		return image;
+
+	}
+	return cv::Mat();
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr GridMap::createTerrainCloud() const
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	if( gridMap_.hasBasicLayers())
+	{
+		const grid_map::Matrix& dataElevation = gridMap_["elevation"];
+		const grid_map::Matrix& dataColors = gridMap_["colors"];
+
+		cloud->width = gridMap_.getSize()(0);
+		cloud->height = gridMap_.getSize()(1);
+		cloud->resize(cloud->width * cloud->height);
+		cloud->is_dense = false;
+
+		float xMin = gridMap_.getPosition().x() - gridMap_.getLength().x()/2.0f;
+		float yMin = gridMap_.getPosition().y() - gridMap_.getLength().y()/2.0f;
+		float cellSize = gridMap_.getResolution();
+
+		for(grid_map::GridMapIterator iterator(gridMap_); !iterator.isPastEnd(); ++iterator)
+		{
+			const grid_map::Index index(*iterator);
+			const float& value = dataElevation(index(0), index(1));
+			const int* color = (const int*)&dataColors(index(0), index(1));
+			const grid_map::Index imageIndex(iterator.getUnwrappedIndex());
+			pcl::PointXYZRGB & pt = cloud->at(cloud->width-1-imageIndex(0), imageIndex(1));
+			if (std::isfinite(value))
+			{
+				pt.x = xMin + (cloud->width-1-imageIndex(0)) * cellSize;
+				pt.y = yMin + (cloud->height-1-imageIndex(1)) * cellSize;
+				pt.z = value;
+				pt.b = (unsigned char)(*color & 0xFF);
+				pt.g = (unsigned char)((*color >> 8) & 0xFF);
+				pt.r = (unsigned char)((*color >> 16) & 0xFF);
+			}
+			else
+			{
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			}
+		}
+	}
+	return cloud;
+}
+
+pcl::PolygonMesh::Ptr GridMap::createTerrainMesh() const
+{
+	pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = createTerrainCloud();
+	if(!cloud->empty())
+	{
+		mesh->polygons = util3d::organizedFastMesh(
+				cloud,
+				M_PI,
+				true,
+				1);
+
+		pcl::toPCLPointCloud2(*cloud, mesh->cloud);
+	}
+
+	return mesh;
+}
+
+void GridMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
+{
+	UTimer timer;
+
+	float margin = cellSize_*10.0f;
+
+	float minX=-minMapSize_/2.0f;
+	float minY=-minMapSize_/2.0f;
+	float maxX=minMapSize_/2.0f;
+	float maxY=minMapSize_/2.0f;
+	bool undefinedSize = minMapSize_ == 0.0f;
+	std::map<int, cv::Mat> occupiedLocalMaps;
+
+	if(gridMap_.hasBasicLayers())
+	{
+		// update
+		minX=minValues_[0]+margin+cellSize_/2.0f;
+		minY=minValues_[1]+margin+cellSize_/2.0f;
+		maxX=minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - margin;
+		maxY=minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - margin;
+		undefinedSize = false;
+	}
+
+	for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
+	{
+		UASSERT(!iter->second.isNull());
+
+		float x = iter->second.x();
+		float y  =iter->second.y();
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+	}
+
+	if(!cache().empty())
+	{
+		UDEBUG("Updating from cache");
+		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
+		{
+			if(uContains(cache(), iter->first))
+			{
+				const LocalGrid & localGrid = cache().at(iter->first);
+
+				if(!localGrid.is3D())
+				{
+					UWARN("It seems the local occupancy grids are not 3d, cannot update GridMap! (ground type=%d, obstacles type=%d, empty type=%d)",
+							localGrid.groundCells.type(), localGrid.obstacleCells.type(), localGrid.emptyCells.type());
+					continue;
+				}
+
+				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
+
+				//ground
+				cv::Mat occupied;
+				if(localGrid.groundCells.cols || localGrid.obstacleCells.cols)
+				{
+					occupied = cv::Mat(1, localGrid.groundCells.cols+localGrid.obstacleCells.cols, CV_32FC4);
+				}
+				if(localGrid.groundCells.cols)
+				{
+					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
+					}
+					for(int i=0; i<localGrid.groundCells.cols; ++i)
+					{
+						const float * vi = localGrid.groundCells.ptr<float>(0,i);
+						float * vo = occupied.ptr<float>(0,i);
+						cv::Point3f vt;
+						vo[3] = 0xFFFFFFFF; // RGBA
+						if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.channels() != 5)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+							if(localGrid.groundCells.channels() == 4)
+							{
+								vo[3] = vi[3];
+							}
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						vo[2] = vt.z;
+						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];
+					}
+				}
+
+				//obstacles
+				if(localGrid.obstacleCells.cols)
+				{
+					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
+					}
+					for(int i=0; i<localGrid.obstacleCells.cols; ++i)
+					{
+						const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
+						float * vo = occupied.ptr<float>(0,i+localGrid.groundCells.cols);
+						cv::Point3f vt;
+						vo[3] = 0xFFFFFFFF; // RGBA
+						if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.channels() != 5)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+							if(localGrid.obstacleCells.channels() == 4)
+							{
+								vo[3] = vi[3];
+							}
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						vo[2] = vt.z;
+						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(occupiedLocalMaps, std::make_pair(iter->first, occupied));
+			}
+		}
+	}
+
+	if(minX != maxX && minY != maxY)
+	{
+		//Get map size
+		float xMin = minX-margin;
+		xMin -= cellSize_/2.0f;
+		float yMin = minY-margin;
+		yMin -= cellSize_/2.0f;
+		float xMax = maxX+margin;
+		float yMax = maxY+margin;
+
+		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
+		   fabs((xMax - xMin) / cellSize_) > 99999)
+		{
+			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
+					"There's maybe an error with the poses provided! The map will not be created!",
+					xMin, yMin, xMax, yMax);
+		}
+		else
+		{
+			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
+			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
+			if(!gridMap_.hasBasicLayers())
+			{
+				UDEBUG("Map empty!");
+				grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
+				grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
+
+				UDEBUG("length: %f, %f position: %f, %f", length[0], length[1], position[0], position[1]);
+				gridMap_.setGeometry(length, cellSize_, position);
+				UDEBUG("size: %d, %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+				// Add elevation layer
+				gridMap_.add("elevation");
+				gridMap_.add("node_ids");
+				gridMap_.add("colors");
+				gridMap_.setBasicLayers({"elevation"});
+			}
+			else
+			{
+				if(xMin == minValues_[0] && yMin == minValues_[1] &&
+						newMapSize.width == gridMap_.getSize()[0] &&
+						newMapSize.height == gridMap_.getSize()[1])
+				{
+					// same map size and origin, don't do anything
+					UDEBUG("Map same size!");
+				}
+				else
+				{
+					UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
+					UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
+					UASSERT_MSG(xMax >= minValues_[0]+float(gridMap_.getSize()[0])*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], gridMap_.getSize()[0], cellSize_).c_str());
+					UASSERT_MSG(yMax >= minValues_[1]+float(gridMap_.getSize()[1])*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], gridMap_.getSize()[1], cellSize_).c_str());
+
+					UDEBUG("Copy map");
+					// copy the old map in the new map
+					// make sure the translation is cellSize
+					int deltaX = 0;
+					if(xMin < minValues_[0])
+					{
+						deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
+						xMin = minValues_[0]-float(deltaX)*cellSize_;
+					}
+					int deltaY = 0;
+					if(yMin < minValues_[1])
+					{
+						deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
+						yMin = minValues_[1]-float(deltaY)*cellSize_;
+					}
+					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
+					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
+					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
+					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], newMapSize.width, newMapSize.height);
+					UASSERT(newMapSize.width >= gridMap_.getSize()[0] && newMapSize.height >= gridMap_.getSize()[1]);
+					UASSERT(newMapSize.width >= gridMap_.getSize()[0]+deltaX && newMapSize.height >= gridMap_.getSize()[1]+deltaY);
+					UASSERT(deltaX>=0 && deltaY>=0);
+
+					grid_map::Length length = grid_map::Length(xMax - xMin, yMax - yMin);
+					grid_map::Position position = grid_map::Position((xMax+xMin)/2.0f, (yMax+yMin)/2.0f);
+
+					grid_map::GridMap tmpExtendedMap;
+					tmpExtendedMap.setGeometry(length, cellSize_, position);
+
+					UDEBUG("%d/%d -> %d/%d", gridMap_.getSize()[0], gridMap_.getSize()[1], tmpExtendedMap.getSize()[0], tmpExtendedMap.getSize()[1]);
+
+					UDEBUG("extendToInclude (%f,%f,%f,%f) -> (%f,%f,%f,%f)",
+							gridMap_.getLength()[0], gridMap_.getLength()[1],
+							gridMap_.getPosition()[0], gridMap_.getPosition()[1],
+							tmpExtendedMap.getLength()[0], tmpExtendedMap.getLength()[1],
+							tmpExtendedMap.getPosition()[0], tmpExtendedMap.getPosition()[1]);
+					if(!gridMap_.extendToInclude(tmpExtendedMap))
+					{
+						UERROR("Failed to update size of the grid map");
+					}
+					UDEBUG("Updated side: %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+				}
+			}
+			UDEBUG("map %d %d", gridMap_.getSize()[0], gridMap_.getSize()[1]);
+			if(newPoses.size())
+			{
+				UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
+			}
+			grid_map::Matrix& gridMapData = gridMap_["elevation"];
+			grid_map::Matrix& gridMapNodeIds = gridMap_["node_ids"];
+			grid_map::Matrix& gridMapColors = gridMap_["colors"];
+			for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
+			{
+				std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(kter->first);
+				if(iter!=occupiedLocalMaps.end())
+				{
+					addAssembledNode(kter->first, kter->second);
+
+					for(int i=0; i<iter->second.cols; ++i)
+					{
+						float * ptf = iter->second.ptr<float>(0,i);
+						grid_map::Position position(ptf[0], ptf[1]);
+						grid_map::Index index;
+						if(gridMap_.getIndex(position, index))
+						{
+							// If no elevation has been set, use current elevation.
+							if (!gridMap_.isValid(index))
+							{
+								gridMapData(index(0), index(1)) = ptf[2];
+								gridMapNodeIds(index(0), index(1)) = kter->first;
+								gridMapColors(index(0), index(1)) = ptf[3];
+							}
+							else
+							{
+								if ((gridMapData(index(0), index(1)) < ptf[2] && (gridMapNodeIds(index(0), index(1)) <= kter->first || kter->first == -1)) ||
+									gridMapNodeIds(index(0), index(1)) < kter->first)
+								{
+									gridMapData(index(0), index(1)) = ptf[2];
+									gridMapNodeIds(index(0), index(1)) = kter->first;
+									gridMapColors(index(0), index(1)) = ptf[3];
+								}
+							}
+						}
+						else
+						{
+							UERROR("Outside map!? (%d) (%f,%f) -> (%d,%d)", i, ptf[0], ptf[1], index[0], index[1]);
+						}
+					}
+				}
+			}
+
+			minValues_[0] = xMin;
+			minValues_[1] = yMin;
+		}
+	}
+	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
+}
+
+}

corelib/src/global_map/OccupancyGrid.cpp

@@ -0,0 +1,682 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+OccupancyGrid::OccupancyGrid(const LocalGridCache * cache, const ParametersMap & parameters) :
+	GlobalMap(cache, parameters),
+	minMapSize_(Parameters::defaultGridGlobalMinSize()),
+	erode_(Parameters::defaultGridGlobalEroded()),
+	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius())
+{
+	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
+	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
+	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
+
+	UASSERT(minMapSize_ >= 0.0f);
+}
+
+void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses)
+{
+	UDEBUG("map=%d/%d xMin=%f yMin=%f cellSize=%f poses=%d",
+			map.cols, map.rows, xMin, yMin, cellSize, (int)poses.size());
+	this->clear();
+	if(!poses.empty() && !map.empty())
+	{
+		UASSERT(cellSize > 0.0f);
+		UASSERT(map.type() == CV_8SC1);
+		map_ = map.clone();
+		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
+		for(int i=0; i<map_.rows; ++i)
+		{
+			for(int j=0; j<map_.cols; ++j)
+			{
+				const char value = map_.at<char>(i,j);
+				float * info = mapInfo_.ptr<float>(i,j);
+				if(value == 0)
+				{
+					info[3] = logOddsClampingMin_;
+				}
+				else if(value == 100)
+				{
+					info[3] = logOddsClampingMax_;
+				}
+			}
+		}
+		minValues_[0] = xMin;
+		minValues_[1] = yMin;
+		cellSize_ = cellSize;
+		addAssembledNode(poses.lower_bound(1)->first, poses.lower_bound(1)->second);
+	}
+}
+
+void OccupancyGrid::clear()
+{
+	map_ = cv::Mat();
+	mapInfo_ = cv::Mat();
+	cellCount_.clear();
+	GlobalMap::clear();
+}
+
+cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
+{
+	xMin = minValues_[0];
+	yMin = minValues_[1];
+
+	cv::Mat map = map_;
+
+	UTimer t;
+	if(occupancyThr_ != 0.0f && !map.empty())
+	{
+		float occThr = logodds(occupancyThr_);
+		map = cv::Mat(map.size(), map.type());
+		UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else if(info[3] >= occThr)
+				{
+					map.at<char>(i, j) = 100; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = 0; // empty
+				}
+			}
+		}
+		UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
+	}
+
+	if(erode_ && !map.empty())
+	{
+		map = util3d::erodeMap(map);
+		UDEBUG("Eroding map = %fs", t.ticks());
+	}
+	return map;
+}
+
+cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
+{
+	xMin = minValues_[0];
+	yMin = minValues_[1];
+
+	cv::Mat map;
+	if(!mapInfo_.empty())
+	{
+		map = cv::Mat(mapInfo_.size(), map_.type());
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
+				}
+			}
+		}
+	}
+	else
+	{
+		UWARN("Map info is empty, cannot generate probabilistic occupancy grid");
+	}
+	return map;
+}
+
+void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPoses)
+{
+	UTimer timer;
+
+	float margin = cellSize_*10.0f+(footprintRadius_>cellSize_*1.5f?float(int(footprintRadius_/cellSize_)+1):0.0f)*cellSize_;
+
+	float minX=-minMapSize_/2.0f;
+	float minY=-minMapSize_/2.0f;
+	float maxX=minMapSize_/2.0f;
+	float maxY=minMapSize_/2.0f;
+	bool undefinedSize = minMapSize_ == 0.0f;
+	std::map<int, cv::Mat> emptyLocalMaps;
+	std::map<int, cv::Mat> occupiedLocalMaps;
+
+	if(!map_.empty())
+	{
+		// update
+		minX=minValues_[0]+margin+cellSize_/2.0f;
+		minY=minValues_[1]+margin+cellSize_/2.0f;
+		maxX=minValues_[0]+float(map_.cols)*cellSize_ - margin;
+		maxY=minValues_[1]+float(map_.rows)*cellSize_ - margin;
+		undefinedSize = false;
+	}
+
+	for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
+	{
+		UASSERT(!iter->second.isNull());
+
+		float x = iter->second.x();
+		float y  =iter->second.y();
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+	}
+
+	if(!cache().empty())
+	{
+		UDEBUG("Updating from cache");
+		for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
+		{
+			if(uContains(cache(), iter->first))
+			{
+				const LocalGrid & localGrid = cache().at(iter->first);
+
+				UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
+
+				//ground
+				cv::Mat ground;
+				if(localGrid.groundCells.cols || localGrid.emptyCells.cols)
+				{
+					ground = cv::Mat(1, localGrid.groundCells.cols+localGrid.emptyCells.cols, CV_32FC2);
+				}
+				if(localGrid.groundCells.cols)
+				{
+					if(localGrid.groundCells.rows > 1 && localGrid.groundCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.groundCells.rows, localGrid.groundCells.cols);
+					}
+					for(int i=0; i<localGrid.groundCells.cols; ++i)
+					{
+						const float * vi = localGrid.groundCells.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(localGrid.groundCells.channels() != 2 && localGrid.groundCells.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];
+					}
+				}
+
+				//empty
+				if(localGrid.emptyCells.cols)
+				{
+					if(localGrid.emptyCells.rows > 1 && localGrid.emptyCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.emptyCells.rows, localGrid.emptyCells.cols);
+					}
+					for(int i=0; i<localGrid.emptyCells.cols; ++i)
+					{
+						const float * vi = localGrid.emptyCells.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i+localGrid.groundCells.cols);
+						cv::Point3f vt;
+						if(localGrid.emptyCells.channels() != 2 && localGrid.emptyCells.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));
+
+				//obstacles
+				if(localGrid.obstacleCells.cols)
+				{
+					if(localGrid.obstacleCells.rows > 1 && localGrid.obstacleCells.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", localGrid.obstacleCells.rows, localGrid.obstacleCells.cols);
+					}
+					cv::Mat obstacles(1, localGrid.obstacleCells.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = localGrid.obstacleCells.ptr<float>(0,i);
+						float * vo = obstacles.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(localGrid.obstacleCells.channels() != 2 && localGrid.obstacleCells.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(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
+				}
+			}
+		}
+	}
+
+	cv::Mat map;
+	cv::Mat mapInfo;
+	if(minX != maxX && minY != maxY)
+	{
+		//Get map size
+		float xMin = minX-margin;
+		xMin -= cellSize_/2.0f;
+		float yMin = minY-margin;
+		yMin -= cellSize_/2.0f;
+		float xMax = maxX+margin;
+		float yMax = maxY+margin;
+
+		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
+		   fabs((xMax - xMin) / cellSize_) > 99999)
+		{
+			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
+					"There's maybe an error with the poses provided! The map will not be created!",
+					xMin, yMin, xMax, yMax);
+		}
+		else
+		{
+			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, minValues_[0], minValues_[1], xMax, yMax);
+			cv::Size newMapSize((xMax - xMin) / cellSize_+0.5f, (yMax - yMin) / cellSize_+0.5f);
+			if(map_.empty())
+			{
+				UDEBUG("Map empty!");
+				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
+			}
+			else
+			{
+				if(xMin == minValues_[0] && yMin == minValues_[1] &&
+						newMapSize.width == map_.cols &&
+						newMapSize.height == map_.rows)
+				{
+					// same map size and origin, don't do anything
+					UDEBUG("Map same size!");
+					map = map_;
+					mapInfo = mapInfo_;
+				}
+				else
+				{
+					UASSERT_MSG(xMin <= minValues_[0]+cellSize_/2, uFormat("xMin=%f, xMin_=%f, cellSize_=%f", xMin, minValues_[0], cellSize_).c_str());
+					UASSERT_MSG(yMin <= minValues_[1]+cellSize_/2, uFormat("yMin=%f, yMin_=%f, cellSize_=%f", yMin, minValues_[1], cellSize_).c_str());
+					UASSERT_MSG(xMax >= minValues_[0]+float(map_.cols)*cellSize_ - cellSize_/2, uFormat("xMin=%f, xMin_=%f, cols=%d cellSize_=%f", xMin, minValues_[0], map_.cols, cellSize_).c_str());
+					UASSERT_MSG(yMax >= minValues_[1]+float(map_.rows)*cellSize_ - cellSize_/2, uFormat("yMin=%f, yMin_=%f, cols=%d cellSize_=%f", yMin, minValues_[1], map_.rows, cellSize_).c_str());
+
+					UDEBUG("Copy map");
+					// copy the old map in the new map
+					// make sure the translation is cellSize
+					int deltaX = 0;
+					if(xMin < minValues_[0])
+					{
+						deltaX = (minValues_[0] - xMin) / cellSize_ + 1.0f;
+						xMin = minValues_[0]-float(deltaX)*cellSize_;
+					}
+					int deltaY = 0;
+					if(yMin < minValues_[1])
+					{
+						deltaY = (minValues_[1] - yMin) / cellSize_ + 1.0f;
+						yMin = minValues_[1]-float(deltaY)*cellSize_;
+					}
+					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
+					newMapSize.width = (xMax - xMin) / cellSize_+0.5f;
+					newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
+					UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
+					UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
+					UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
+					UASSERT(deltaX>=0 && deltaY>=0);
+					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
+					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+					mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+				}
+			}
+			UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
+			UDEBUG("map %d %d", map.cols, map.rows);
+			if(newPoses.size())
+			{
+				UDEBUG("first pose= %d last pose=%d", newPoses.begin()->first, newPoses.rbegin()->first);
+			}
+			for(std::list<std::pair<int, Transform> >::const_iterator kter = newPoses.begin(); kter!=newPoses.end(); ++kter)
+			{
+				std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
+				std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
+				if(iter != emptyLocalMaps.end() || jter!=occupiedLocalMaps.end())
+				{
+					addAssembledNode(kter->first, kter->second);
+					std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+					if(cter == cellCount_.end() && kter->first > 0)
+					{
+						cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
+					}
+					if(iter!=emptyLocalMaps.end())
+					{
+						for(int i=0; i<iter->second.cols; ++i)
+						{
+							float * ptf = iter->second.ptr<float>(0,i);
+							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+							UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
+									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
+							char & value = map.at<char>(pt.y, pt.x);
+							if(value != -2)
+							{
+								float * info = mapInfo.ptr<float>(pt.y, pt.x);
+								int nodeId = (int)info[0];
+								if(value != -1)
+								{
+									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									{
+										// cannot rewrite on cells referred by more recent nodes
+										continue;
+									}
+									if(nodeId > 0)
+									{
+										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+										if(value == 0)
+										{
+											eter->second.first -= 1;
+										}
+										else if(value == 100)
+										{
+											eter->second.second -= 1;
+										}
+										if(kter->first < 0)
+										{
+											eter->second.first += 1;
+										}
+									}
+								}
+								if(kter->first > 0)
+								{
+									info[0] = (float)kter->first;
+									info[1] = ptf[0];
+									info[2] = ptf[1];
+									cter->second.first+=1;
+								}
+								value = 0; // free space
+
+								// update odds
+								if(nodeId != kter->first)
+								{
+									info[3] += logOddsMiss_;
+									if (info[3] < logOddsClampingMin_)
+									{
+										info[3] = logOddsClampingMin_;
+									}
+									if (info[3] > logOddsClampingMax_)
+									{
+										info[3] = logOddsClampingMax_;
+									}
+								}
+							}
+						}
+					}
+
+					if(footprintRadius_ >= cellSize_*1.5f)
+					{
+						// place free space under the footprint of the robot
+						cv::Point2i ptBegin((kter->second.x()-footprintRadius_-xMin)/cellSize_, (kter->second.y()-footprintRadius_-yMin)/cellSize_);
+						cv::Point2i ptEnd((kter->second.x()+footprintRadius_-xMin)/cellSize_, (kter->second.y()+footprintRadius_-yMin)/cellSize_);
+						if(ptBegin.x < 0)
+							ptBegin.x = 0;
+						if(ptEnd.x >= map.cols)
+							ptEnd.x = map.cols-1;
+
+						if(ptBegin.y < 0)
+							ptBegin.y = 0;
+						if(ptEnd.y >= map.rows)
+							ptEnd.y = map.rows-1;
+
+						for(int i=ptBegin.x; i<ptEnd.x; ++i)
+						{
+							for(int j=ptBegin.y; j<ptEnd.y; ++j)
+							{
+								UASSERT(j < map.rows && i < map.cols);
+								char & value = map.at<char>(j, i);
+								float * info = mapInfo.ptr<float>(j, i);
+								int nodeId = (int)info[0];
+								if(value != -1)
+								{
+									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									{
+										// cannot rewrite on cells referred by more recent nodes
+										continue;
+									}
+									if(nodeId>0)
+									{
+										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+										if(value == 0)
+										{
+											eter->second.first -= 1;
+										}
+										else if(value == 100)
+										{
+											eter->second.second -= 1;
+										}
+										if(kter->first < 0)
+										{
+											eter->second.first += 1;
+										}
+									}
+								}
+								if(kter->first > 0)
+								{
+									info[0] = (float)kter->first;
+									info[1] = float(i) * cellSize_ + xMin;
+									info[2] = float(j) * cellSize_ + yMin;
+									info[3] = logOddsClampingMin_;
+									cter->second.first+=1;
+								}
+								value = -2; // free space (footprint)
+							}
+						}
+					}
+
+					if(jter!=occupiedLocalMaps.end())
+					{
+						for(int i=0; i<jter->second.cols; ++i)
+						{
+							float * ptf = jter->second.ptr<float>(0,i);
+							cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
+							UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
+										uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+												kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
+							char & value = map.at<char>(pt.y, pt.x);
+							if(value != -2)
+							{
+								float * info = mapInfo.ptr<float>(pt.y, pt.x);
+								int nodeId = (int)info[0];
+								if(value != -1)
+								{
+									if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+									{
+										// cannot rewrite on cells referred by more recent nodes
+										continue;
+									}
+									if(nodeId>0)
+									{
+										std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+										UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+										if(value == 0)
+										{
+											eter->second.first -= 1;
+										}
+										else if(value == 100)
+										{
+											eter->second.second -= 1;
+										}
+										if(kter->first < 0)
+										{
+											eter->second.second += 1;
+										}
+									}
+								}
+								if(kter->first > 0)
+								{
+									info[0] = (float)kter->first;
+									info[1] = ptf[0];
+									info[2] = ptf[1];
+									cter->second.second+=1;
+								}
+
+								// update odds
+								if(nodeId != kter->first || value!=100)
+								{
+									info[3] += logOddsHit_;
+									if (info[3] < logOddsClampingMin_)
+									{
+										info[3] = logOddsClampingMin_;
+									}
+									if (info[3] > logOddsClampingMax_)
+									{
+										info[3] = logOddsClampingMax_;
+									}
+								}
+
+								value = 100; // obstacles
+							}
+						}
+					}
+				}
+			}
+
+			if(footprintRadius_ >= cellSize_*1.5f)
+			{
+				for(int i=1; i<map.rows-1; ++i)
+				{
+					for(int j=1; j<map.cols-1; ++j)
+					{
+						char & value = map.at<char>(i, j);
+						if(value == -2)
+						{
+							value = 0;
+						}
+					}
+				}
+			}
+
+			map_ = map;
+			mapInfo_ = mapInfo;
+			minValues_[0] = xMin;
+			minValues_[1] = yMin;
+
+			// clean cellCount_
+			for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
+			{
+				UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
+				if(iter->second.first == 0 && iter->second.second == 0)
+				{
+					cellCount_.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+		}
+	}
+
+	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
+}
+
+unsigned long OccupancyGrid::getMemoryUsed() const
+{
+	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
+
+	memoryUsage += map_.total() * map_.elemSize();
+	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
+	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::map<int, std::pair<int, int> >::iterator)) + sizeof(std::map<int, std::pair<int, int> >);
+
+	return memoryUsage;
+}
+
+}

corelib/src/global_map/OctoMap.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <rtabmap/core/OctoMap.h>
+#include <rtabmap/core/global_map/OctoMap.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -288,55 +288,40 @@ RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTr
 // OctoMap
 //////////////////////////////////////
 
-OctoMap::OctoMap(const ParametersMap & parameters) :
+OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) :
+		GlobalMap(cache, parameters),
 		hasColor_(false),
-		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
-		updateError_(Parameters::defaultGridGlobalUpdateError()),
 		rangeMax_(Parameters::defaultGridRangeMax()),
 		rayTracing_(Parameters::defaultGridRayTracing()),
 		emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
 {
-	float cellSize = Parameters::defaultGridCellSize();
-	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
-	UASSERT(cellSize>0.0f);
-
-	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
-	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
-
-	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
-	float probHit = Parameters::defaultGridGlobalProbHit();
-	float probMiss = Parameters::defaultGridGlobalProbMiss();
-	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
-	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
-	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
-	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
-	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
-	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
-	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
-
-	octree_ = new RtabmapColorOcTree(cellSize);
-	if(occupancyThr <= 0.0f)
+	octree_ = new RtabmapColorOcTree(cellSize_);
+	if(occupancyThr_ <= 0.0f)
 	{
 		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
 				Parameters::kGridGlobalOccupancyThr().c_str(),
 				Parameters::defaultGridGlobalOccupancyThr());
-		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+		occupancyThr_ = Parameters::defaultGridGlobalOccupancyThr();
 	}
-	octree_->setOccupancyThres(occupancyThr);
-	octree_->setProbHit(probHit);
-	octree_->setProbMiss(probMiss);
-	octree_->setClampingThresMin(clampingMin);
-	octree_->setClampingThresMax(clampingMax);
-	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
-   
-	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
+	
+	UDEBUG("occupancyThr_=%f", occupancyThr_);
+	UDEBUG("probHit_=%f", probability(logOddsHit_));
+	UDEBUG("probMiss_=%f", probability(logOddsMiss_));
+	UDEBUG("probClampingMin_=%f", probability(logOddsClampingMin_));
+	UDEBUG("probClampingMax_=%f", probability(logOddsClampingMax_));
+	
+	octree_->setOccupancyThres(occupancyThr_);
+	octree_->setProbHit(probability(logOddsHit_));
+	octree_->setProbMiss(probability(logOddsMiss_));
+	octree_->setClampingThresMin(probability(logOddsClampingMin_));
+	octree_->setClampingThresMax(probability(logOddsClampingMax_));
+
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
 	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
+
 	Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
 	UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);
 
-	UDEBUG("fullUpdate_         =%s", fullUpdate_?"true":"false");
-	UDEBUG("updateError_        =%f", updateError_);
 	UDEBUG("rangeMax_           =%f", rangeMax_);
 	UDEBUG("rayTracing_         =%s", rayTracing_?"true":"false");
 	UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
@@ -351,47 +336,17 @@ OctoMap::~OctoMap()
 void OctoMap::clear()
 {
 	octree_->clear();
-	cache_.clear();
-	cacheClouds_.clear();
-	cacheViewPoints_.clear();
-	addedNodes_.clear();
 	hasColor_ = false;
-	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
-	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+	GlobalMap::clear();
 }
 
-void OctoMap::addToCache(int nodeId,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
-		const pcl::PointXYZ & viewPoint)
+unsigned long OctoMap::getMemoryUsed() const
 {
-	UDEBUG("nodeId=%d", nodeId);
-	if(nodeId < 0)
-	{
-		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
-		return;
-	}
-	cacheClouds_.erase(nodeId==0?-1:nodeId);
-	cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles)));
-	uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
-}
-void OctoMap::addToCache(int nodeId,
-		const cv::Mat & ground,
-		const cv::Mat & obstacles,
-		const cv::Mat & empty,
-		const cv::Point3f & viewPoint)
-{
-	UDEBUG("nodeId=%d", nodeId);
-	if(nodeId < 0)
-	{
-		UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
-		return;
-	}
-	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());
-	uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
-	uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
+	unsigned long memoryUsage = GlobalMap::getMemoryUsed();
+
+	// Note: size of OctoMap object is missing.
+
+	return memoryUsage;
 }
 
 bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
@@ -509,227 +464,67 @@ std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::fin
 }
 
 
-bool OctoMap::update(const std::map<int, Transform> & poses)
+void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
 {
-	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
-
-	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
-	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;
-	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);
-		if(jter != poses.end())
-		{
-			graphChanged = false;
-			UASSERT(!iter->second.isNull() && !jter->second.isNull());
-			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
-			{
-				t = jter->second * iter->second.inverse();
-				graphOptimized = true;
-			}
-			transforms.insert(std::make_pair(jter->first, t));
-			updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
-		}
-		else
-		{
-			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
-		}
-	}
-
-	if(graphOptimized || graphChanged)
-	{
-		if(graphChanged)
-		{
-			UWARN("Graph has changed! The whole map should be rebuilt.");
-		}
-		else
-		{
-			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();
-			addedNodes_.clear();
-			hasColor_ = false;
-		}
-		else
-		{
-			RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
-			int copied=0;
-			int count=0;
-			UTimer t;
-			for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
-			{
-				RtabmapColorOcTreeNode & nOld = *it;
-				if(nOld.getNodeRefId() > 0)
-				{
-					std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
-					if(jter != transforms.end())
-					{
-						octomap::point3d pt;
-						std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
-						UASSERT(pter != addedNodes_.end());
-
-						if(nOld.getOccupancyType() > 0)
-						{
-							pt = nOld.getPointRef();
-						}
-						else
-						{
-							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 if(jter == transforms.end())
-					{
-						// 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());
-					}
-				}
-			}
-			UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
-			delete octree_;
-			octree_ = newOcTree;
-
-			//update added poses
-			addedNodes_ = updatedAddedNodes;
-		}
-	}
-
 	// Original version from A. Hornung:
 	// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
 	//
-	std::list<std::pair<int, Transform> > orderedPoses;
 
-	int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
+	int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0;
 	UDEBUG("Last id = %d", lastId);
 
-	// add old poses that were not in the current map (they were just retrieved from LTM)
-	for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
-	{
-		if(addedNodes_.find(iter->first) == addedNodes_.end())
-		{
-			orderedPoses.push_back(*iter);
-		}
-	}
+	UDEBUG("newPoses = %d", (int)newPoses.size());
 
-	// insert zero after
-	if(poses.find(0) != poses.end())
-	{
-		orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
-	}
-
-	UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
-
-	if(!orderedPoses.empty())
+	if(!newPoses.empty())
 	{
 		float rangeMaxSqrd = rangeMax_*rangeMax_;
 		float cellSize = octree_->getResolution();
-		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
+		for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 		{
-			std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-			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);
-			viewPointIter = cacheViewPoints_.find(iter->first);
-			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
+			std::map<int, LocalGrid>::const_iterator localGridIter;
+			localGridIter = cache().find(iter->first);
+			if(localGridIter != cache().end())
 			{
+				cv::Mat ground = localGridIter->second.groundCells;
+				cv::Mat obstacles = localGridIter->second.obstacleCells;
+				cv::Mat emptyCells = localGridIter->second.emptyCells;
+
+				if(!localGridIter->second.is3D())
+				{
+					UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
+											ground.type(), obstacles.type(), emptyCells.type());
+					continue;
+				}
+
 				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
 
-				UASSERT(viewPointIter != cacheViewPoints_.end());
 				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
-				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+				sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z);
 
 				updateMinMax(sensorOrigin);
 
 				octomap::OcTreeKey tmpKey;
-				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
-						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
+				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
 				{
 					UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 				}
 
-				bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());
+				bool computeRays = rayTracing_ && emptyCells.empty();
 
 				// instead of direct scan insertion, compute update to filter ground:
 				octomap::KeySet free_cells;
 				// insert ground points only as free:
-				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
+				unsigned int maxGroundPts = ground.cols;
 				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
 				Eigen::Affine3f t = iter->second.toEigen3f();
-				LaserScan tmpGround;
-				if(occupancyIter != cache_.end())
-				{
-					tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
-					UASSERT(tmpGround.size() == (int)maxGroundPts);
-				}
+				LaserScan tmpGround = LaserScan::backwardCompatibility(ground);
+				UASSERT(tmpGround.size() == (int)maxGroundPts);
 				for (unsigned int i=0; i<maxGroundPts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(tmpGround, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
-					}
+					pt = util3d::laserScanToPointRGB(tmpGround, i);
+					pt = pcl::transformPoint(pt, t);
+
 					octomap::point3d point(pt.x, pt.y, pt.z);
 					bool ignoreOccupiedCell = false;
 					if(rangeMaxSqrd > 0.0f)
@@ -793,26 +588,15 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 				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.first.second.cols:cloudIter->second.second->size();
+				unsigned int maxObstaclePts = obstacles.cols;
 				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
-				LaserScan tmpObstacle;
-				if(occupancyIter != cache_.end())
-				{
-					tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
-					UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
-				}
+				LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles);
+				UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
 				for (unsigned int i=0; i<maxObstaclePts; ++i)
 				{
 					pcl::PointXYZRGB pt;
-					if(occupancyIter != cache_.end())
-					{
-						pt = util3d::laserScanToPointRGB(tmpObstacle, i);
-						pt = pcl::transformPoint(pt, t);
-					}
-					else
-					{
-						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
-					}
+					pt = util3d::laserScanToPointRGB(tmpObstacle, i);
+					pt = pcl::transformPoint(pt, t);
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -903,11 +687,11 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 				}
 
 				// all empty cells
-				if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
+				if(emptyCells.cols)
 				{
-					unsigned int maxEmptyPts = occupancyIter->second.second.cols;
+					unsigned int maxEmptyPts = emptyCells.cols;
 					UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
-					LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
+					LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells);
 					UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
 					for (unsigned int i=0; i<maxEmptyPts; ++i)
 					{
@@ -959,22 +743,18 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
 					}
 				}
 
-				if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
+				if(emptyCells.cols || !free_cells.empty())
 				{
 					octree_->updateInnerOccupancy();
 				}
 
 				// compress map
-				//if(orderedPoses.size() > 1)
+				//if(newPoses.size() > 1)
 				//{
 				//	octree_->prune();
 				//}
 
-				// ignore negative ids as they are temporary clouds
-				if(iter->first > 0)
-				{
-					addedNodes_.insert(*iter);
-				}
+				addAssembledNode(iter->first, iter->second);
 				UDEBUG("%d: end", iter->first);
 			}
 			else
@@ -1005,21 +785,12 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
             }
         }
 
-
         for(unsigned int y=0; y < nodeToDelete.size(); y++)
         {
             octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
         }
         UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
     }
-
-	if(!fullUpdate_)
-	{
-		cache_.clear();
-		cacheClouds_.clear();
-		cacheViewPoints_.clear();
-	}
-	return !orderedPoses.empty() || graphOptimized || graphChanged || emptyFloodFillDepth_>0;
 }
 
 void OctoMap::updateMinMax(const octomap::point3d & point)