Parameters: added Grid/FullUpdate (default true). Updated how occupancy grid is updated after loop closure. OctoMap: added tree depth argument when creating 2d map, added full update argument on constructor (default false). MainWindow: using OccupancyGrid object instead of keeping in cache local grids (we can have actual time to update the global grid).

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -45,6 +45,8 @@ public:
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
+	bool isFullUpdate() const {return fullUpdate_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
@@ -104,6 +106,7 @@ private:
 	bool scan2dUnknownSpaceFilled_;
 	double scan2dMaxUnknownSpaceFilledRange_;
 	bool projRayTracing_;
+	bool fullUpdate_;
 
 	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
 	cv::Mat map_;

corelib/include/rtabmap/core/OctoMap.h

@@ -57,7 +57,7 @@ public:
 
 class RTABMAP_EXP OctoMap {
 public:
-	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f);
+	OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
@@ -81,7 +81,8 @@ public:
 			float & xMin,
 			float & yMin,
 			float & gridCellSize,
-			float minGridSize);
+			float minGridSize = 0.0f,
+			unsigned int treeDepth = 0);
 
 	bool writeBinary(const std::string & path);
 
@@ -97,6 +98,7 @@ private:
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
 	bool hasColor_;
+	bool fullUpdate_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -505,6 +505,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
 	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   4.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
 	RTABMAP_PARAM(Grid, ProjRayTracing,               bool,   true,    uFormat("[%s=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.", kGrid3D().c_str()));
+	RTABMAP_PARAM(Grid, FullUpdate,                   bool,   true,    "When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not.");
 
 public:
 	virtual ~Parameters();

corelib/include/rtabmap/core/util3d_mapping.h

@@ -116,6 +116,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
+cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
+
 template<typename PointT>
 typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 		const typename pcl::PointCloud<PointT> & cloud);