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

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 15)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_MINOR_VERSION 16)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/DBDriver.h

@@ -92,6 +92,7 @@ public:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
@@ -216,6 +217,7 @@ private:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint) const = 0;
 

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -48,6 +48,7 @@ public:
 	float getMinMapSize() const {return minMapSize_;}
 	bool isGridFromDepth() const {return occupancyFromCloud_;}
 	bool isFullUpdate() const {return fullUpdate_;}
+	float getUpdateError() const {return updateError_;}
 	bool isMapFrameProjection() const {return projMapFrame_;}
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	int cacheSize() const {return (int)cache_.size();}
@@ -64,19 +65,38 @@ public:
 
 	void createLocalMap(
 			const Signature & node,
-			cv::Mat & ground,
-			cv::Mat & obstacles,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint) const;
 
+	void createLocalMap(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+	void createLocalMap(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+			const pcl::IndicesPtr & indices,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
 	void clear();
 	void addToCache(
 			int nodeId,
 			const cv::Mat & ground,
-			const cv::Mat & obstacles);
+			const cv::Mat & obstacles,
+			const cv::Mat & empty);
 	void update(const std::map<int, Transform> & poses);
 	cv::Mat getMap(float & xMin, float & yMin) const;
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
 private:
 	ParametersMap parameters_;
@@ -106,13 +126,14 @@ private:
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;
 	double scan2dMaxUnknownSpaceFilledRange_;
-	bool projRayTracing_;
+	bool rayTracing_;
 	bool fullUpdate_;
 	float minMapSize_;
 	bool erode_;
 	float footprintRadius_;
+	float updateError_;
 
-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_; //<node id, <ground, obstacles> >
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;
 	cv::Mat mapInfo_;
 	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
@@ -123,6 +144,7 @@ private:
 	bool cloudAssembling_;
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledEmptyCells_;
 };
 
 }

corelib/include/rtabmap/core/OctoMap.h

@@ -44,22 +44,132 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class OcTreeNodeInfo
+// forward declaraton for "friend"
+class RtabmapColorOcTree;
+
+class RtabmapColorOcTreeNode : public octomap::ColorOcTreeNode
 {
 public:
-	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
-		nodeRefId_(nodeRefId),
-		key_(key),
-		isObstacle_(isObstacle) {}
+	enum OccupancyType {kTypeUnknown=-1, kTypeEmpty=0, kTypeGround=1, kTypeObstacle=100};
+
+public:
+	friend class RtabmapColorOcTree; // needs access to node children (inherited)
+
+	RtabmapColorOcTreeNode() : ColorOcTreeNode(), nodeRefId_(0), type_(-1) {}
+	RtabmapColorOcTreeNode(const RtabmapColorOcTreeNode& rhs) : ColorOcTreeNode(rhs), nodeRefId_(rhs.nodeRefId_), type_(rhs.type_) {}
+
+	void setNodeRefId(int nodeRefId) {nodeRefId_ = nodeRefId;}
+	void setOccupancyType(char type) {type_=type;}
+	void setPointRef(const octomap::point3d & point) {pointRef_ = point;}
+	int getNodeRefId() const {return nodeRefId_;}
+	char getOccupancyType() const {return type_;}
+	const octomap::point3d & getPointRef() const {return pointRef_;}
+
+private:
 	int nodeRefId_;
-	octomap::OcTreeKey key_;
-	bool isObstacle_;
+	char type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
+	octomap::point3d pointRef_;
 };
 
+// Same as official ColorOctree but using RtabmapColorOcTreeNode, which is inheriting ColorOcTreeNode
+class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTreeNode> {
+
+  public:
+    /// Default constructor, sets resolution of leafs
+	RtabmapColorOcTree(double resolution);
+
+    /// virtual constructor: creates a new object of same type
+    /// (Covariant return type requires an up-to-date compiler)
+	RtabmapColorOcTree* create() const {return new RtabmapColorOcTree(resolution); }
+
+    std::string getTreeType() const {return "ColorOcTree";} // same type as ColorOcTree to be compatible with ROS OctoMap msg
+
+     /**
+     * Prunes a node when it is collapsible. This overloaded
+     * version only considers the node occupancy for pruning,
+     * different colors of child nodes are ignored.
+     * @return true if pruning was successful
+     */
+    virtual bool pruneNode(RtabmapColorOcTreeNode* node);
+
+    virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode* node) const;
+
+    // set node color at given key or coordinate. Replaces previous color.
+    RtabmapColorOcTreeNode* setNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                 uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* setNodeColor(float x, float y,
+                                 float z, uint8_t r,
+                                 uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return setNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* averageNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* averageNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap:: OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return averageNodeColor(key,r,g,b);
+    }
+
+    // integrate color measurement at given key or coordinate. Average with previous color
+    RtabmapColorOcTreeNode* integrateNodeColor(const octomap::OcTreeKey& key, uint8_t r,
+                                  uint8_t g, uint8_t b);
+
+    RtabmapColorOcTreeNode* integrateNodeColor(float x, float y,
+                                      float z, uint8_t r,
+                                      uint8_t g, uint8_t b) {
+    	octomap::OcTreeKey key;
+      if (!this->coordToKeyChecked(octomap::point3d(x,y,z), key)) return NULL;
+      return integrateNodeColor(key,r,g,b);
+    }
+
+    // update inner nodes, sets color to average child color
+    void updateInnerOccupancy();
+
+  protected:
+    void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth);
+
+    /**
+     * Static member object which ensures that this OcTree's prototype
+     * ends up in the classIDMapping only once. You need this as a
+     * static member in any derived octree class in order to read .ot
+     * files through the AbstractOcTree factory. You should also call
+     * ensureLinking() once from the constructor.
+     */
+    class StaticMemberInitializer{
+       public:
+         StaticMemberInitializer() {
+        	 RtabmapColorOcTree* tree = new RtabmapColorOcTree(0.1);
+           tree->clearKeyRays();
+           AbstractOcTree::registerTreeType(tree);
+         }
+
+         /**
+         * Dummy function to ensure that MSVC does not drop the
+         * StaticMemberInitializer, causing this tree failing to register.
+         * Needs to be called from the constructor of this octree.
+         */
+         void ensureLinking() {};
+    };
+    /// static member to ensure static initialization (only once)
+    static StaticMemberInitializer RtabmapColorOcTreeMemberInit;
+
+  };
+
 class RTABMAP_EXP OctoMap {
+public:
+	static void HSVtoRGB(float *r, float *g, float *b, float h, float s, float v);
+
 public:
 	OctoMap(const ParametersMap & parameters, float occupancyThr = 0.5f);
-	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
+	OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
 
 	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
 	void addToCache(int nodeId,
@@ -69,15 +179,18 @@ public:
 	void addToCache(int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			const cv::Point3f & viewPoint);
 	void update(const std::map<int, Transform> & poses);
 
-	const octomap::ColorOcTree * octree() const {return octree_;}
+	const RtabmapColorOcTree * octree() const {return octree_;}
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
 			unsigned int treeDepth = 0,
 			std::vector<int> * obstacleIndices = 0,
-			std::vector<int> * emptyIndices = 0) const;
+			std::vector<int> * emptyIndices = 0,
+			std::vector<int> * groundIndices = 0,
+			bool originalRefPoints = true) const;
 
 	cv::Mat createProjectionMap(
 			float & xMin,
@@ -91,16 +204,24 @@ public:
 	virtual ~OctoMap();
 	void clear();
 
+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
 private:
-	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	void updateMinMax(const octomap::point3d & point);
+
+private:
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; // [id: < <ground, obstacles>, empty>]
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_; // [id: <ground, obstacles>]
 	std::map<int, cv::Point3f> cacheViewPoints_;
-	octomap::ColorOcTree * octree_;
-	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	RtabmapColorOcTree * octree_;
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
 	bool hasColor_;
 	bool fullUpdate_;
+	float updateError_;
+	double minValues_[3];
+	double maxValues_[3];
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -617,9 +617,10 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
     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, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
 
     RTABMAP_PARAM(GridGlobal, 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.");
+    RTABMAP_PARAM(GridGlobal, UpdateError,          float,  0.01,    "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
     RTABMAP_PARAM(GridGlobal, FootprintRadius,      float,  0.0,     "Footprint radius (m) used to clear all obstacles under the graph.");
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");

corelib/include/rtabmap/core/SensorData.h

@@ -175,14 +175,16 @@ public:
 			cv::Mat * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0);
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0);
 	void uncompressDataConst(
 			cv::Mat * imageRaw,
 			cv::Mat * depthOrRightRaw,
 			cv::Mat * laserScanRaw = 0,
 			cv::Mat * userDataRaw = 0,
 			cv::Mat * groundCellsRaw = 0,
-			cv::Mat * obstacleCellsRaw = 0) const;
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
@@ -203,6 +205,7 @@ public:
 	void setOccupancyGrid(
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint);
 	// remove raw occupancy grids
@@ -211,6 +214,8 @@ public:
 	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
 	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
 	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
+	const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
 	float gridCellSize() const {return _cellSize;}
 	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
 
@@ -261,8 +266,10 @@ private:
 	// occupancy grid
 	cv::Mat _groundCellsCompressed;
 	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _emptyCellsCompressed;
 	cv::Mat _groundCellsRaw;
 	cv::Mat _obstacleCellsRaw;
+	cv::Mat _emptyCellsRaw;
 	float _cellSize;
 	cv::Point3f _viewPoint;
 

corelib/include/rtabmap/core/util3d.h

@@ -203,6 +203,7 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNorm
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
 // return CV_32FC4 (x,y,z,rgb)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform());
 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
 // return CV_32FC7 (x,y,z,rgb,normal_x,normal_y,normal_z)

corelib/include/rtabmap/core/util3d_mapping.h

@@ -45,8 +45,8 @@ namespace util3d
 
 RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scan, // in /base_link frame
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f), "Use interface with \"viewpoint\" parameter to make sure the ray tracing origin is from the sensor and not the base.");
@@ -54,8 +54,8 @@ RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 RTABMAP_DEPRECATED(void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scan, // in /base_link frame
 		const cv::Point3f & viewpoint, // /base_link -> /base_scan
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f), "Use interface with scanHit/scanNoHit parameters: scanNoHit set to null matrix has the same functionality than this method.");
@@ -64,8 +64,8 @@ void RTABMAP_EXP occupancy2DFromLaserScan(
 		const cv::Mat & scanHit, // in /base_link frame
 		const cv::Mat & scanNoHit, // in /base_link frame
 		const cv::Point3f & viewpoint, // /base_link -> /base_scan
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled = false,
 		float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true

corelib/src/DBDriver.cpp

@@ -475,17 +475,19 @@ void DBDriver::updateOccupancyGrid(
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		float cellSize,
 		const cv::Point3f & viewpoint)
 {
 	_dbSafeAccessMutex.lock();
 	//just to make sure the occupancy grids are compressed for convenience
 	SensorData data;
-	data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
+	data.setOccupancyGrid(ground, obstacles, empty, cellSize, viewpoint);
 	this->updateOccupancyGridQuery(
 			nodeId,
 			data.gridGroundCellsCompressed(),
 			data.gridObstacleCellsCompressed(),
+			data.gridEmptyCellsCompressed(),
 			cellSize,
 			viewpoint);
 	_dbSafeAccessMutex.unlock();

corelib/src/DBDriverSqlite3.cpp

@@ -1220,7 +1220,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 			}
 			if(occupancyGrid)
 			{
-				fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				if(uStrNumCmp(_version, "0.16.0") >= 0)
+				{
+					fields << "ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				}
+				else
+				{
+					fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
+				}
 			}
 
 			query << "SELECT " << fields.str().c_str() << " "
@@ -1557,6 +1564,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				// Occupancy grid
 				cv::Mat groundCellsCompressed;
 				cv::Mat obstacleCellsCompressed;
+				cv::Mat emptyCellsCompressed;
 				float cellSize = 0.0f;
 				cv::Point3f viewPoint;
 				if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
@@ -1579,6 +1587,18 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
 					}
 
+					if(uStrNumCmp(_version, "0.16.0") >= 0)
+					{
+						// empty
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if(dataSize > 0 && data)
+						{
+							emptyCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
+							memcpy((void*)emptyCellsCompressed.data, data, dataSize);
+						}
+					}
+
 					cellSize = sqlite3_column_double(ppStmt, index++);
 					viewPoint.x = sqlite3_column_double(ppStmt, index++);
 					viewPoint.y = sqlite3_column_double(ppStmt, index++);
@@ -1612,11 +1632,11 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				}
 				if(occupancyGrid)
 				{
-					(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, cellSize, viewPoint);
+					(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, emptyCellsCompressed, cellSize, viewPoint);
 				}
 				else
 				{
-					(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
+					(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridEmptyCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
 				}
 				rc = sqlite3_step(ppStmt); // next result...
 			}
@@ -3881,6 +3901,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const
 {
@@ -3903,6 +3924,7 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 				nodeId,
 				ground,
 				obstacles,
+				empty,
 				cellSize,
 				viewpoint);
 
@@ -4834,7 +4856,11 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.11.10") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -5046,6 +5072,21 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		}
 
+		if(uStrNumCmp(_version, "0.16.0") >= 0)
+		{
+			//empty_cells
+			if(sensorData.gridEmptyCellsCompressed().empty())
+			{
+				rc = sqlite3_bind_null(ppStmt, index++);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+			else
+			{
+				rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridEmptyCellsCompressed().data, (int)sensorData.gridEmptyCellsCompressed().cols, SQLITE_STATIC);
+				UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+			}
+		}
+
 		//cell_size
 		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -5308,22 +5349,29 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		return "UPDATE Data SET ground_cells=?, obstacle_cells=?, empty_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
+	}
 	return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
 }
 void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 		int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		float cellSize,
 		const cv::Point3f & viewpoint) const
 {
 	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
 	UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
 	UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
-	UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d cell=%f viewpoint=(%f,%f,%f)",
+	UASSERT(empty.empty() || empty.type() == CV_8UC1); // compressed
+	UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d empty=%d cell=%f viewpoint=(%f,%f,%f)",
 			nodeId,
 			ground.cols,
 			obstacles.cols,
+			empty.cols,
 			cellSize,
 			viewpoint.x,
 			viewpoint.y,
@@ -5361,6 +5409,21 @@ void DBDriverSqlite3::stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 	}
 
+	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	{
+		//empty_cells
+		if(empty.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, empty.data, empty.cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
 	//cell_size
 	rc = sqlite3_bind_double(ppStmt, index++, cellSize);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

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

corelib/src/Memory.cpp

@@ -4043,19 +4043,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	// Occupancy grid map stuff
-	cv::Mat ground, obstacles;
+	cv::Mat ground, obstacles, empty;
 	float cellSize = 0.0f;
 	cv::Point3f viewPoint(0,0,0);
 	if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
 	{
-		_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
+		_occupancy->createLocalMap(*s, ground, obstacles, empty, viewPoint);
 		cellSize = _occupancy->getCellSize();
 
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
 		UDEBUG("time grid map = %fs", t);
 	}
-	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
+	s->sensorData().setOccupancyGrid(ground, obstacles, empty, cellSize, viewPoint);
 
 	// prior
 	if(!isIntermediateNode)

corelib/src/OccupancyGrid.cpp

@@ -32,6 +32,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
 
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#endif
+
 #include <pcl/io/pcd_io.h>
 
 namespace rtabmap {
@@ -64,16 +68,18 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
 	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
-	projRayTracing_(Parameters::defaultGridProjRayTracing()),
+	rayTracing_(Parameters::defaultGridRayTracing()),
 	fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
 	minMapSize_(Parameters::defaultGridGlobalMinSize()),
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
+	updateError_(Parameters::defaultGridGlobalUpdateError()),
 	xMin_(0.0f),
 	yMin_(0.0f),
 	cloudAssembling_(false),
 	assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
-	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>)
+	assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
+	assembledEmptyCells_(new pcl::PointCloud<pcl::PointXYZRGB>)
 {
 	this->parseParameters(parameters);
 }
@@ -117,11 +123,12 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
 	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
-	Parameters::parse(parameters, Parameters::kGridProjRayTracing(), projRayTracing_);
+	Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalMinSize(), minMapSize_);
 	Parameters::parse(parameters, Parameters::kGridGlobalEroded(), erode_);
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 
 	UASSERT(minMapSize_ >= 0.0f);
 
@@ -213,8 +220,9 @@ void OccupancyGrid::setCloudAssembling(bool enabled)
 
 void OccupancyGrid::createLocalMap(
 		const Signature & node,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
 		cv::Point3f & viewPoint) const
 {
 	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
@@ -233,11 +241,13 @@ void OccupancyGrid::createLocalMap(
 				util3d::transformLaserScan(node.sensorData().laserScanRaw(), node.sensorData().laserScanInfo().localTransform()),
 				cv::Mat(),
 				viewPoint,
-				ground,
-				obstacles,
+				emptyCells,
+				obstacleCells,
 				cellSize_,
 				scan2dUnknownSpaceFilled_,
 				node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
+
+		UDEBUG("ground=%d obstacles=%d channels=%d", emptyCells.cols, obstacleCells.cols, obstacleCells.cols?obstacleCells.channels():emptyCells.channels());
 	}
 	else
 	{
@@ -300,94 +310,149 @@ void OccupancyGrid::createLocalMap(
 				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
 			}
 		}
+		createLocalMap(cloud, indices, node.getPose(), groundCells, obstacleCells, emptyCells, viewPoint);
+	}
+}
 
-		if(projMapFrame_)
+void OccupancyGrid::createLocalMap(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	UASSERT_MSG(cloud->size() && cloud->is_dense, uFormat("Use interface with indices if cloud is not dense.").c_str());
+	createLocalMap(cloud, indices, pose, groundCells, obstacleCells, emptyCells, viewPointInOut);
+}
+
+void OccupancyGrid::createLocalMap(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, // in base_link frame
+		const pcl::IndicesPtr & indices,
+		const Transform & pose,
+		cv::Mat & groundCells,
+		cv::Mat & obstacleCells,
+		cv::Mat & emptyCells,
+		cv::Point3f & viewPointInOut) const
+{
+	if(projMapFrame_)
+	{
+		//we should rotate viewPoint in /map frame
+		float roll, pitch, yaw;
+		pose.getEulerAngles(roll, pitch, yaw);
+		Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z, 0,0,0);
+		viewPointInOut.x = viewpointRotated.x();
+		viewPointInOut.y = viewpointRotated.y();
+		viewPointInOut.z = viewpointRotated.z();
+	}
+
+	if((cloud->is_dense && cloud->size()) ||
+		(!cloud->is_dense && indices->size()))
+	{
+		pcl::IndicesPtr groundIndices(new std::vector<int>);
+		pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudSegmented = this->segmentCloud<pcl::PointXYZRGB>(
+				cloud,
+				indices,
+				pose,
+				viewPointInOut,
+				groundIndices,
+				obstaclesIndices);
+
+		if(!groundIndices->empty() || !obstaclesIndices->empty())
 		{
-			//we should rotate viewPoint in /map frame
-			float roll, pitch, yaw;
-			node.getPose().getEulerAngles(roll, pitch, yaw);
-			Transform viewpointRotated = Transform(0,0,0,roll,pitch,0) * Transform(viewPoint.x, viewPoint.y, viewPoint.z, 0,0,0);
-			viewPoint.x = viewpointRotated.x();
-			viewPoint.y = viewpointRotated.y();
-			viewPoint.z = viewpointRotated.z();
-		}
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-		if((cloud->is_dense && cloud->size()) ||
-			(!cloud->is_dense && indices->size()))
-		{
-			pcl::IndicesPtr groundIndices(new std::vector<int>);
-			pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
-			cloud = this->segmentCloud<pcl::PointXYZRGB>(
-					cloud,
-					indices,
-					node.getPose(),
-					viewPoint,
-					groundIndices,
-					obstaclesIndices);
-
-			if(!groundIndices->empty() || !obstaclesIndices->empty())
+			if(groundIndices->size())
 			{
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-				pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				pcl::copyPointCloud(*cloudSegmented, *groundIndices, *groundCloud);
+			}
 
-				if(groundIndices->size())
+			if(obstaclesIndices->size())
+			{
+				pcl::copyPointCloud(*cloudSegmented, *obstaclesIndices, *obstaclesCloud);
+			}
+
+			if(grid3D_)
+			{
+				UDEBUG("");
+				if(groundIsObstacle_)
 				{
-					pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+					*obstaclesCloud += *groundCloud;
+					groundCloud->clear();
 				}
 
-				if(obstaclesIndices->size())
-				{
-					pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-				}
+				// transform back in base frame
+				float roll, pitch, yaw;
+				pose.getEulerAngles(roll, pitch, yaw);
+				Transform tinv = Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0).inverse();
 
-				if(grid3D_)
+				if(rayTracing_)
 				{
-					UDEBUG("");
-					if(groundIsObstacle_)
+#ifdef RTABMAP_OCTOMAP
+					if(!groundCloud->empty() || !obstaclesCloud->empty())
 					{
-						*obstaclesCloud += *groundCloud;
-						groundCloud->clear();
-					}
+						//create local octomap
+						OctoMap octomap(cellSize_);
+						octomap.addToCache(1, groundCloud, obstaclesCloud, pcl::PointXYZ(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
+						std::map<int, Transform> poses;
+						poses.insert(std::make_pair(1, Transform::getIdentity()));
+						octomap.update(poses);
 
-					// transform back in base frame
-					float roll, pitch, yaw;
-					node.getPose().getEulerAngles(roll, pitch, yaw);
-					Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
-					ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
-					obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+						obstaclesIndices->clear();
+						groundIndices->clear();
+						pcl::IndicesPtr emptyIndices(new std::vector<int>);
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithRayTracing = octomap.createCloud(0, obstaclesIndices.get(), emptyIndices.get(), groundIndices.get());
+						UDEBUG("ground=%d obstacles=%d empty=%d", (int)groundIndices->size(), (int)obstaclesIndices->size(), (int)emptyIndices->size());
+						groundCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, groundIndices, tinv);
+						obstacleCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, obstaclesIndices, tinv);
+						emptyCells = util3d::laserScanFromPointCloud(*cloudWithRayTracing, emptyIndices, tinv);
+					}
 				}
 				else
+#else
+					UWARN("RTAB-Map is not built with OctoMap dependency, 3D ray tracing is ignored. Set \"%s\" to false to avoid this warning.", Parameters::kGridRayTracing().c_str());
+				}
+#endif
 				{
-					UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
-					// projection on the xy plane
-					util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-							groundCloud,
-							obstaclesCloud,
-							ground,
-							obstacles,
-							cellSize_);
+					groundCells = util3d::laserScanFromPointCloud(*groundCloud, tinv);
+					obstacleCells = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+				}
 
-					if(projRayTracing_)
-					{
-						cv::Mat laserScan = obstacles;
-						cv::Mat laserScanNoHit = ground;
-						obstacles = cv::Mat();
-						ground = cv::Mat();
-						util3d::occupancy2DFromLaserScan(
-								laserScan,
-								laserScanNoHit,
-								viewPoint,
-								ground,
-								obstacles,
-								cellSize_,
-								false, // don't fill unknown space
-								0);
-					}
+			}
+			else
+			{
+				UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+				// projection on the xy plane
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+						groundCloud,
+						obstaclesCloud,
+						groundCells,
+						obstacleCells,
+						cellSize_);
+
+				if(rayTracing_)
+				{
+					cv::Mat laserScan = obstacleCells;
+					cv::Mat laserScanNoHit = groundCells;
+					obstacleCells = cv::Mat();
+					groundCells = cv::Mat();
+					util3d::occupancy2DFromLaserScan(
+							laserScan,
+							laserScanNoHit,
+							viewPointInOut,
+							emptyCells,
+							obstacleCells,
+							cellSize_,
+							false, // don't fill unknown space
+							0);
 				}
 			}
 		}
 	}
-	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
+	UDEBUG("ground=%d obstacles=%d empty=%d, channels=%d", groundCells.cols, obstacleCells.cols, emptyCells.cols, obstacleCells.cols?obstacleCells.channels():groundCells.channels());
 }
 
 void OccupancyGrid::clear()
@@ -417,10 +482,11 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 void OccupancyGrid::addToCache(
 		int nodeId,
 		const cv::Mat & ground,
-		const cv::Mat & obstacles)
+		const cv::Mat & obstacles,
+		const cv::Mat & empty)
 {
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
 }
 
 void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
@@ -442,6 +508,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	std::map<int, Transform> transforms;
+	float updateErrorSqrd = updateError_*updateError_;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
@@ -451,7 +518,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
 			{
 				t = jter->second * iter->second.inverse();
 				graphOptimized = true;
@@ -487,6 +554,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 	bool assembledGroundUpdated = false;
 	bool assembledObstaclesUpdated = false;
+	bool assembledEmptyCellsUpdated = false;
 
 	if(graphOptimized || graphChanged)
 	{
@@ -676,22 +744,22 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		{
 			if(uContains(cache_, iter->first))
 			{
-				const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
+				const std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> & pair = cache_.at(iter->first);
 
 				//ground
-				if(pair.first.cols)
+				if(pair.first.first.cols)
 				{
-					if(pair.first.rows > 1 && pair.first.cols == 1)
+					if(pair.first.first.rows > 1 && pair.first.first.cols == 1)
 					{
-						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.first.rows, pair.first.first.cols);
 					}
-					cv::Mat ground(1, pair.first.cols, CV_32FC2);
+					cv::Mat ground(1, pair.first.first.cols, CV_32FC2);
 					for(int i=0; i<ground.cols; ++i)
 					{
-						const float * vi = pair.first.ptr<float>(0,i);
+						const float * vi = pair.first.first.ptr<float>(0,i);
 						float * vo = ground.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() != 2 && pair.first.channels() != 5)
+						if(pair.first.first.channels() != 2 && pair.first.first.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -715,25 +783,67 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 					if(cloudAssembling_)
 					{
-						*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first, iter->second, 0, 255, 0);
+						*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first.first, iter->second, 0, 255, 0);
 						assembledGroundUpdated = true;
 					}
 				}
 
-				//obstacles
+				//empty
 				if(pair.second.cols)
 				{
 					if(pair.second.rows > 1 && pair.second.cols == 1)
 					{
 						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
 					}
-					cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
-					for(int i=0; i<obstacles.cols; ++i)
+					cv::Mat ground(1, pair.second.cols, CV_32FC2);
+					for(int i=0; i<ground.cols; ++i)
 					{
 						const float * vi = pair.second.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.second.channels() != 2 && pair.second.channels() != 5)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+
+					if(cloudAssembling_)
+					{
+						*assembledEmptyCells_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 0, 255, 0);
+						assembledEmptyCellsUpdated = true;
+					}
+				}
+
+				//obstacles
+				if(pair.first.second.cols)
+				{
+					if(pair.first.second.rows > 1 && pair.first.second.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.second.rows, pair.first.second.cols);
+					}
+					cv::Mat obstacles(1, pair.first.second.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = pair.first.second.ptr<float>(0,i);
 						float * vo = obstacles.ptr<float>(0,i);
 						cv::Point3f vt;
-						if(pair.first.channels() != 2 && pair.first.channels() != 5)
+						if(pair.first.second.channels() != 2 && pair.first.second.channels() != 5)
 						{
 							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
 						}
@@ -757,7 +867,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 
 					if(cloudAssembling_)
 					{
-						*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 255, 0, 0);
+						*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.first.second, iter->second, 255, 0, 0);
 						assembledObstaclesUpdated = true;
 					}
 				}
@@ -1165,6 +1275,10 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 		{
 			assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
 		}
+		if(assembledEmptyCellsUpdated && assembledEmptyCells_->size() > 1)
+		{
+			assembledEmptyCells_ = util3d::voxelize(assembledEmptyCells_, cellSize_);
+		}
 	}
 
 	if(!fullUpdate_ && !cloudAssembling_)
@@ -1174,7 +1288,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	else
 	{
 		//clear only negative ids
-		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
+		for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
 		{
 			if(iter->first < 0)
 			{

corelib/src/OctoMap.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OctoMap.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
@@ -35,24 +36,159 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+//////////////////////////////////////
+// RtabmapColorOcTree
+//////////////////////////////////////
+
+RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
+	: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
+	RtabmapColorOcTreeMemberInit.ensureLinking();
+};
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search (key);
+	if (n != 0) {
+		n->setColor(r, g, b);
+	}
+	return n;
+}
+
+bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
+	if (!isNodeCollapsible(node))
+		return false;
+
+	// set value to children's values (all assumed equal)
+	node->copyData(*(getNodeChild(node, 0)));
+
+	if (node->isColorSet()) // TODO check
+		node->setColor(node->getAverageChildColor());
+
+	// delete children
+	for (unsigned int i=0;i<8;i++) {
+		deleteNodeChild(node, i);
+	}
+	delete[] node->children;
+	node->children = NULL;
+
+	return true;
+}
+
+bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
+	// all children must exist, must not have children of
+	// their own and have the same occupancy probability
+	if (!nodeChildExists(node, 0))
+		return false;
+
+	const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
+	if (nodeHasChildren(firstChild))
+		return false;
+
+	for (unsigned int i = 1; i<8; i++) {
+		// compare nodes only using their occupancy, ignoring color for pruning
+		if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
+			return false;
+	}
+
+	return true;
+}
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search(key);
+	if (n != 0) {
+		if (n->isColorSet()) {
+			RtabmapColorOcTreeNode::Color prev_color = n->getColor();
+			n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
+		}
+		else {
+			n->setColor(r, g, b);
+		}
+	}
+	return n;
+}
+
+RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
+		uint8_t r,
+		uint8_t g,
+		uint8_t b) {
+	RtabmapColorOcTreeNode* n = search (key);
+	if (n != 0) {
+		if (n->isColorSet()) {
+			RtabmapColorOcTreeNode::Color prev_color = n->getColor();
+			double node_prob = n->getOccupancy();
+			uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
+					+  (double) r * (0.99-node_prob));
+			uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
+					+  (double) g * (0.99-node_prob));
+			uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
+					+  (double) b * (0.99-node_prob));
+			n->setColor(new_r, new_g, new_b);
+		}
+		else {
+			n->setColor(r, g, b);
+		}
+	}
+	return n;
+}
+
+
+void RtabmapColorOcTree::updateInnerOccupancy() {
+	this->updateInnerOccupancyRecurs(this->root, 0);
+}
+
+void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
+	// only recurse and update for inner nodes:
+	if (nodeHasChildren(node)){
+		// return early for last level:
+		if (depth < this->tree_depth){
+			for (unsigned int i=0; i<8; i++) {
+				if (nodeChildExists(node, i)) {
+					updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
+				}
+			}
+		}
+		node->updateOccupancyChildren();
+		node->updateColorChildren();
+	}
+}
+
+
+//////////////////////////////////////
+// OctoMap
+//////////////////////////////////////
+
 OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
 		hasColor_(false),
-		fullUpdate_(Parameters::defaultGridGlobalFullUpdate())
+		fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
+		updateError_(Parameters::defaultGridGlobalUpdateError())
 {
 	float cellSize = Parameters::defaultGridCellSize();
 	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
 	UASSERT(cellSize>0.0f);
 
-	octree_ = new octomap::ColorOcTree(cellSize);
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
+	octree_ = new RtabmapColorOcTree(cellSize);
 	octree_->setOccupancyThres(occupancyThr);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
+	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 }
 
-OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate) :
-		octree_(new octomap::ColorOcTree(cellSize)),
+OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
+		octree_(new RtabmapColorOcTree(cellSize)),
 		hasColor_(false),
-		fullUpdate_(fullUpdate)
+		fullUpdate_(fullUpdate),
+		updateError_(updateError)
 {
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
 	octree_->setOccupancyThres(occupancyThr);
 	UASSERT(cellSize>0.0f);
 }
@@ -66,13 +202,14 @@ OctoMap::~OctoMap()
 void OctoMap::clear()
 {
 	octree_->clear();
-	occupiedCells_.clear();
 	cache_.clear();
 	cacheClouds_.clear();
 	cacheViewPoints_.clear();
 	addedNodes_.clear();
 	keyRay_ = octomap::KeyRay();
 	hasColor_ = false;
+	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
 }
 
 void OctoMap::addToCache(int nodeId,
@@ -87,12 +224,14 @@ void OctoMap::addToCache(int nodeId,
 void OctoMap::addToCache(int nodeId,
 		const cv::Mat & ground,
 		const cv::Mat & obstacles,
+		const cv::Mat & empty,
 		const cv::Point3f & viewPoint)
 {
-	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
-	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
+	UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
+	UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
+	UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
 	UDEBUG("nodeId=%d", nodeId);
-	uInsert(cache_, std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	uInsert(cache_, std::make_pair(nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
 	uInsert(cacheViewPoints_, std::make_pair(nodeId, viewPoint));
 }
 
@@ -105,6 +244,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
 	std::map<int, Transform> transforms;
 	std::map<int, Transform> updatedAddedNodes;
+	float updateErrorSqrd = updateError_*updateError_;
 	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
 	{
 		std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
@@ -113,7 +253,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			graphChanged = false;
 			UASSERT(!iter->second.isNull() && !jter->second.isNull());
 			Transform t = Transform::getIdentity();
-			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
 			{
 				t = jter->second * iter->second.inverse();
 				graphOptimized = true;
@@ -137,64 +277,99 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			UINFO("Graph optimized!");
 		}
 
+		minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
+		maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
+
 		if(fullUpdate_ || graphChanged)
 		{
 			// clear all but keep cache
 			octree_->clear();
-			occupiedCells_.clear();
 			addedNodes_.clear();
 			keyRay_ = octomap::KeyRay();
 			hasColor_ = false;
 		}
 		else
 		{
-			octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
-			std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
+			RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
 			int copied=0;
-			for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo >::iterator iter = occupiedCells_.begin();
-					iter!=occupiedCells_.end();
-					++iter)
+			int count=0;
+			UTimer t;
+			for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
 			{
-				std::map<int, Transform>::iterator jter = transforms.find(iter->second.nodeRefId_);
-				if(jter != transforms.end())
+				RtabmapColorOcTreeNode & nOld = *it;
+				if(nOld.getNodeRefId() > 0)
 				{
-					octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
-					std::map<int, Transform>::iterator pter = addedNodes_.find(iter->second.nodeRefId_);
-					UASSERT(pter != addedNodes_.end());
-
-					cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
-					cvPt = util3d::transformPoint(cvPt, jter->second);
-
-					octomap::OcTreeKey key;
-					if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
+					std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
+					if(jter != transforms.end())
 					{
-						octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
-						if(n)
+						octomap::point3d pt;
+						std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
+						UASSERT(pter != addedNodes_.end());
+
+						if(nOld.getOccupancyType() > 0)
 						{
-							++copied;
-							uInsert(newOccupiedCells, std::make_pair(n, OcTreeNodeInfo(jter->first, key, iter->second.isObstacle_)));
-							newOcTree->setNodeColor(key, iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+							pt = nOld.getPointRef();
 						}
 						else
 						{
-							UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+							pt = octree_->keyToCoord(it.getKey());
+						}
+
+						cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
+						cvPt = util3d::transformPoint(cvPt, jter->second);
+						octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
+
+						octomap::OcTreeKey key;
+						if(newOcTree->coordToKeyChecked(ptTransformed, key))
+						{
+							RtabmapColorOcTreeNode * n = newOcTree->search(key);
+							if(n)
+							{
+								if(n->getNodeRefId() > nOld.getNodeRefId())
+								{
+									// The cell has been updated from more recent node, don't update the cell
+									continue;
+								}
+								else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
+								{
+									// empty cells cannot overwrite ground/obstacle cells
+									continue;
+								}
+							}
+
+							RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
+							if(nNew)
+							{
+								++copied;
+								updateMinMax(ptTransformed);
+								nNew->setNodeRefId(nOld.getNodeRefId());
+								if(nOld.getOccupancyType() > 0)
+								{
+									nNew->setPointRef(pt);
+								}
+								nNew->setOccupancyType(nOld.getOccupancyType());
+								nNew->setColor(nOld.getColor());
+							}
+							else
+							{
+								UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+							}
+						}
+						else
+						{
+							UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
 						}
 					}
-					else
+					else if(jter == transforms.end())
 					{
-						UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
+						// Note: normal if old nodes were transfered to LTM
+						//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
 					}
 				}
-				else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
-				{
-					// Note: normal if old nodes were transfered to LTM
-					//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
-				}
 			}
-			UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
+			UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
 			delete octree_;
 			octree_ = newOcTree;
-			occupiedCells_ = newOccupiedCells;
 
 			//update added poses
 			addedNodes_ = updatedAddedNodes;
@@ -235,7 +410,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 	{
 		std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-		std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
+		std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
 		std::map<int, cv::Point3f>::iterator viewPointIter;
 		cloudIter = cacheClouds_.find(iter->first);
 		occupancyIter = cache_.find(iter->first);
@@ -248,6 +423,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
 			sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
 
+			updateMinMax(sensorOrigin);
+
 			octomap::OcTreeKey tmpKey;
 			if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
 					|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -255,10 +432,12 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
 			}
 
+			bool computeRays = occupancyIter == cache_.end() || occupancyIter->second.second.empty();
+
 			// instead of direct scan insertion, compute update to filter ground:
-			octomap::KeySet free_cells, occupied_cells, ground_cells;
+			octomap::KeySet free_cells;
 			// insert ground points only as free:
-			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
+			unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
 			UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
 			Eigen::Affine3f t = iter->second.toEigen3f();
 			for (unsigned int i=0; i<maxGroundPts; ++i)
@@ -266,7 +445,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				pcl::PointXYZRGB pt;
 				if(occupancyIter != cache_.end())
 				{
-					pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first.first, i);
 					pt = pcl::transformPoint(pt, t);
 				}
 				else
@@ -277,7 +456,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				octomap::point3d point(pt.x, pt.y, pt.z);
 
 				// only clear space (ground points)
-				if ((iter->first < 0 || iter->first>lastId) &&
+				if (computeRays &&
+					(iter->first < 0 || iter->first>lastId) &&
 					octree_->computeRayKeys(sensorOrigin, point, keyRay_))
 				{
 					free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -288,17 +468,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				{
 					if(iter->first >0 && iter->first<lastId)
 					{
-						octomap::ColorOcTreeNode * n = octree_->search(key);
-						if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
+						RtabmapColorOcTreeNode * n = octree_->search(key);
+						if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
 						{
 							// The cell has been updated from more recent node, don't update the cell
 							continue;
 						}
 					}
 
-					ground_cells.insert(key);
+					updateMinMax(point);
 
-					octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
 					if(n)
 					{
 						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -308,26 +488,24 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 						if(iter->first > 0)
 						{
-							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
-						}
-						else
-						{
-							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, false)));
+							n->setNodeRefId(iter->first);
+							n->setPointRef(point);
 						}
+						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
 					}
 				}
 			}
-			UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
+			UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
 
 			// all other points: free on ray, occupied on endpoint:
-			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
+			unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
 			UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
 			for (unsigned int i=0; i<maxObstaclePts; ++i)
 			{
 				pcl::PointXYZRGB pt;
 				if(occupancyIter != cache_.end())
 				{
-					pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+					pt = util3d::laserScanToPointRGB(occupancyIter->second.first.second, i);
 					pt = pcl::transformPoint(pt, t);
 				}
 				else
@@ -338,7 +516,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				octomap::point3d point(pt.x, pt.y, pt.z);
 
 				// free cells
-				if ((iter->first < 0 || iter->first>lastId) &&
+				if (computeRays &&
+					(iter->first < 0 || iter->first>lastId) &&
 					octree_->computeRayKeys(sensorOrigin, point, keyRay_))
 				{
 					free_cells.insert(keyRay_.begin(), keyRay_.end());
@@ -349,17 +528,17 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				{
 					if(iter->first >0 && iter->first<lastId)
 					{
-						octomap::ColorOcTreeNode * n = octree_->search(key);
-						if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
+						RtabmapColorOcTreeNode * n = octree_->search(key);
+						if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
 						{
 							// The cell has been updated from more recent node, don't update the cell
 							continue;
 						}
 					}
 
-					occupied_cells.insert(key);
+					updateMinMax(point);
 
-					octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
+					RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
 					if(n)
 					{
 						if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
@@ -369,32 +548,60 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 						if(iter->first > 0)
 						{
-							uInsert(occupiedCells_, std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
-						}
-						else
-						{
-							occupiedCells_.insert(std::make_pair(n, OcTreeNodeInfo(iter->first, key, true)));
+							n->setNodeRefId(iter->first);
+							n->setPointRef(point);
 						}
+						n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
 					}
 				}
 			}
-			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)occupied_cells.size(), (int)free_cells.size());
+			UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
 
 
 			// mark free cells only if not seen occupied in this cloud
 			for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
 			{
-				if (occupied_cells.find(*it) == occupied_cells.end() &&
-					ground_cells.find(*it) == ground_cells.end())
+				RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false);
+				if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
 				{
-					octomap::ColorOcTreeNode * n = octree_->updateNode(*it, false);
-					if(n)
+					n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
+					n->setNodeRefId(iter->first);
+				}
+			}
+
+			// all empty cells
+			if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
+			{
+				unsigned int maxEmptyPts = occupancyIter->second.second.cols;
+				UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
+				for (unsigned int i=0; i<maxEmptyPts; ++i)
+				{
+					pcl::PointXYZ pt;
+					pt = util3d::laserScanToPoint(occupancyIter->second.second, i);
+					pt = pcl::transformPoint(pt, t);
+
+					octomap::point3d point(pt.x, pt.y, pt.z);
+
+					octomap::OcTreeKey key;
+					if (octree_->coordToKeyChecked(point, key))
 					{
-						std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::iterator gter;
-						gter = occupiedCells_.find(n);
-						if(gter != occupiedCells_.end() && gter->second.isObstacle_)
+						updateMinMax(point);
+
+						if(iter->first >0 && iter->first<lastId)
 						{
-							occupiedCells_.erase(gter);
+							RtabmapColorOcTreeNode * n = octree_->search(key);
+							if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
+							{
+								// The cell has been updated from more recent node, don't update the cell
+								continue;
+							}
+						}
+
+						RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
+						if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
+						{
+							n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
+							n->setNodeRefId(iter->first);
 						}
 					}
 				}
@@ -415,6 +622,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 			UDEBUG("Did not find %d in cache", iter->first);
 		}
 	}
+
 	if(!fullUpdate_)
 	{
 		cache_.clear();
@@ -423,7 +631,35 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	}
 }
 
-void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
+void OctoMap::updateMinMax(const octomap::point3d & point)
+{
+	if(point.x() < minValues_[0])
+	{
+		minValues_[0] = point.x();
+	}
+	if(point.y() < minValues_[1])
+	{
+		minValues_[1] = point.y();
+	}
+	if(point.z() < minValues_[2])
+	{
+		minValues_[2] = point.z();
+	}
+	if(point.x() > maxValues_[0])
+	{
+		maxValues_[0] = point.x();
+	}
+	if(point.y() > maxValues_[1])
+	{
+		maxValues_[1] = point.y();
+	}
+	if(point.z() > maxValues_[2])
+	{
+		maxValues_[2] = point.z();
+	}
+}
+
+void OctoMap::HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
 {
 	int i;
 	float f, p, q, t;
@@ -475,7 +711,9 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 		unsigned int treeDepth,
 		std::vector<int> * obstacleIndices,
-		std::vector<int> * emptyIndices) const
+		std::vector<int> * emptyIndices,
+		std::vector<int> * groundIndices,
+		bool originalRefPoints) const
 {
 	UASSERT(treeDepth <= octree_->getTreeDepth());
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -490,22 +728,28 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	{
 		emptyIndices->resize(octree_->size());
 	}
+	if(groundIndices)
+	{
+		groundIndices->resize(octree_->size());
+	}
 
 	if(treeDepth == 0)
 	{
 		treeDepth = octree_->getTreeDepth();
 	}
 
-	double minX, minY, minZ, maxX, maxY, maxZ;
-	octree_->getMetricMin(minX, minY, minZ);
-	octree_->getMetricMax(maxX, maxY, maxZ);
+	double minZ = minValues_[2];
+	double maxZ = maxValues_[2];
 
+	bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
 	int oi=0;
 	int si=0;
+	int ei=0;
 	int gi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
+	float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
+	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
-		if(octree_->isNodeOccupied(*it) && (obstacleIndices || emptyIndices == 0))
+		if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || addAllPoints))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
@@ -522,25 +766,45 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 				(*cloud)[oi].g = g*255.0f;
 				(*cloud)[oi].b = b*255.0f;
 			}
-			(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
-			(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
-			(*cloud)[oi].z = pt.z();
+
+			if(originalRefPoints && it->getOccupancyType() > 0)
+			{
+				const octomap::point3d & p = it->getPointRef();
+				(*cloud)[oi].x = p.x();
+				(*cloud)[oi].y = p.y();
+				(*cloud)[oi].z = p.z();
+			}
+			else
+			{
+				(*cloud)[oi].x = pt.x()-halfCellSize;
+				(*cloud)[oi].y = pt.y()-halfCellSize;
+				(*cloud)[oi].z = pt.z();
+			}
+
 			if(obstacleIndices)
 			{
 				obstacleIndices->at(si++) = oi;
 			}
+
 			++oi;
 		}
-		else if(emptyIndices || obstacleIndices == 0)
+		else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || groundIndices != 0 || addAllPoints))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
 			(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
-			(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
-			(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
+			(*cloud)[oi].x = pt.x()-halfCellSize;
+			(*cloud)[oi].y = pt.y()-halfCellSize;
 			(*cloud)[oi].z = pt.z();
-			if(emptyIndices)
+			if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
 			{
-				emptyIndices->at(gi++) = oi;
+				if(groundIndices)
+				{
+					groundIndices->at(gi++) = oi;
+				}
+			}
+			else if(emptyIndices)
+			{
+				emptyIndices->at(ei++) = oi;
 			}
 			++oi;
 		}
@@ -550,10 +814,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 	if(obstacleIndices)
 	{
 		obstacleIndices->resize(si);
+		UDEBUG("obstacle=%d", si);
 	}
 	if(emptyIndices)
 	{
-		emptyIndices->resize(gi);
+		emptyIndices->resize(ei);
+		UDEBUG("empty=%d", ei);
+	}
+	if(groundIndices)
+	{
+		groundIndices->resize(gi);
+		UDEBUG("ground=%d", gi);
 	}
 
 	UDEBUG("");
@@ -569,6 +840,7 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	}
 
 	gridCellSize = octree_->getNodeSize(treeDepth);
+	float halfCellSize = gridCellSize/2.0f;
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
@@ -577,16 +849,16 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 	obstacles->resize(octree_->size());
 	int gi=0;
 	int oi=0;
-	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
+	for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
 	{
 		octomap::point3d pt = octree_->keyToCoord(it.getKey());
 		if(octree_->isNodeOccupied(*it))
 		{
-			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
+			(*obstacles)[oi++]  = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
 		}
 		else
 		{
-			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
+			(*ground)[gi++]  = pcl::PointXYZ(pt.x()-halfCellSize, pt.y()-halfCellSize, 0); // projected on ground
 		}
 	}
 	obstacles->resize(oi);
@@ -594,11 +866,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 
 	if(obstacles->size())
 	{
-		obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
+		obstacles = util3d::voxelize(obstacles, halfCellSize);
 	}
 	if(ground->size())
 	{
-		ground = util3d::voxelize(ground, gridCellSize/2.0f);
+		ground = util3d::voxelize(ground, halfCellSize);
 	}
 
 	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);

corelib/src/Parameters.cpp

@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	if(removedParameters_.empty())
 	{
 		// removed parameters
+
+		// 0.16.0
+		removedParameters_.insert(std::make_pair("Grid/ProjRayTracing",           std::make_pair(true,  Parameters::kGridRayTracing())));
+
 		// 0.15.1
 		removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount",  std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("Reg/VarianceNormalized",        std::make_pair(false, "")));

corelib/src/Rtabmap.cpp

@@ -2128,7 +2128,7 @@ bool Rtabmap::process(
 									}
 									else
 									{
-										UWARN("Local scan matching rejected: %s", info.rejectedMsg.c_str());
+										UINFO("Local scan matching rejected: %s", info.rejectedMsg.c_str());
 									}
 								}
 							}

corelib/src/SensorData.cpp

@@ -476,12 +476,14 @@ void SensorData::setUserData(const cv::Mat & userData)
 void SensorData::setOccupancyGrid(
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewPoint)
 {
-	UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
+	UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
 	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
-	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
+	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
+	   (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
 	{
 		UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
 		return;
@@ -491,9 +493,12 @@ void SensorData::setOccupancyGrid(
 	_groundCellsCompressed = cv::Mat();
 	_obstacleCellsRaw = cv::Mat();
 	_obstacleCellsCompressed = cv::Mat();
+	_emptyCellsRaw = cv::Mat();
+	_emptyCellsCompressed = cv::Mat();
 
 	CompressionThread ctGround(ground);
 	CompressionThread ctObstacles(obstacles);
+	CompressionThread ctEmpty(empty);
 
 	if(!ground.empty())
 	{
@@ -521,8 +526,22 @@ void SensorData::setOccupancyGrid(
 			_obstacleCellsCompressed = obstacles;
 		}
 	}
+	if(!empty.empty())
+	{
+		if(empty.type() == CV_32FC2 || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(5) || empty.type() == CV_32FC(6) || empty.type() == CV_32FC(7))
+		{
+			_emptyCellsRaw = empty;
+			ctEmpty.start();
+		}
+		else if(empty.type() == CV_8UC1)
+		{
+			UASSERT(empty.type() == CV_8UC1); // Bytes
+			_emptyCellsCompressed = empty;
+		}
+	}
 	ctGround.join();
 	ctObstacles.join();
+	ctEmpty.join();
 	if(!_groundCellsRaw.empty())
 	{
 		_groundCellsCompressed = ctGround.getCompressedData();
@@ -531,6 +550,10 @@ void SensorData::setOccupancyGrid(
 	{
 		_obstacleCellsCompressed = ctObstacles.getCompressedData();
 	}
+	if(!_emptyCellsRaw.empty())
+	{
+		_emptyCellsCompressed = ctEmpty.getCompressedData();
+	}
 
 	_cellSize = cellSize;
 	_viewPoint = viewPoint;
@@ -538,13 +561,14 @@ void SensorData::setOccupancyGrid(
 
 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
+	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF, tmpG;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
 				_laserScanCompressed.empty()?0:&tmpC,
 				_userDataCompressed.empty()?0:&tmpD,
 				_groundCellsCompressed.empty()?0:&tmpE,
-				_obstacleCellsCompressed.empty()?0:&tmpF);
+				_obstacleCellsCompressed.empty()?0:&tmpF,
+				_emptyCellsCompressed.empty()?0:&tmpG);
 }
 
 void SensorData::uncompressData(
@@ -553,15 +577,17 @@ void SensorData::uncompressData(
 		cv::Mat * laserScanRaw,
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
-		cv::Mat * obstacleCellsRaw)
+		cv::Mat * obstacleCellsRaw,
+		cv::Mat * emptyCellsRaw)
 {
-	UDEBUG("%d data(%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0);
+	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
 		userDataRaw == 0 &&
 		groundCellsRaw == 0 &&
-		obstacleCellsRaw == 0)
+		obstacleCellsRaw == 0 &&
+		emptyCellsRaw == 0)
 	{
 		return;
 	}
@@ -571,7 +597,8 @@ void SensorData::uncompressData(
 			laserScanRaw,
 			userDataRaw,
 			groundCellsRaw,
-			obstacleCellsRaw);
+			obstacleCellsRaw,
+			emptyCellsRaw);
 
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
@@ -609,6 +636,10 @@ void SensorData::uncompressData(
 	{
 		_obstacleCellsRaw = *obstacleCellsRaw;
 	}
+	if(emptyCellsRaw && !emptyCellsRaw->empty() && _emptyCellsRaw.empty())
+	{
+		_emptyCellsRaw = *emptyCellsRaw;
+	}
 }
 
 void SensorData::uncompressDataConst(
@@ -617,7 +648,8 @@ void SensorData::uncompressDataConst(
 		cv::Mat * laserScanRaw,
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
-		cv::Mat * obstacleCellsRaw) const
+		cv::Mat * obstacleCellsRaw,
+		cv::Mat * emptyCellsRaw) const
 {
 	if(imageRaw)
 	{
@@ -643,12 +675,17 @@ void SensorData::uncompressDataConst(
 	{
 		*obstacleCellsRaw = _obstacleCellsRaw;
 	}
+	if(emptyCellsRaw)
+	{
+		*emptyCellsRaw = _emptyCellsRaw;
+	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()) ||
 		(userDataRaw && userDataRaw->empty()) ||
 		(groundCellsRaw && groundCellsRaw->empty()) ||
-		(obstacleCellsRaw && obstacleCellsRaw->empty()))
+		(obstacleCellsRaw && obstacleCellsRaw->empty()) ||
+		(emptyCellsRaw && emptyCellsRaw->empty()))
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
@@ -656,6 +693,7 @@ void SensorData::uncompressDataConst(
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
 		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
 		rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
+		rtabmap::CompressionThread ctEmptyCells(_emptyCellsCompressed, false);
 		if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
 		{
 			UASSERT(_imageCompressed.type() == CV_8UC1);
@@ -686,12 +724,18 @@ void SensorData::uncompressDataConst(
 			UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
 			ctObstacleCells.start();
 		}
+		if(emptyCellsRaw && emptyCellsRaw->empty() && !_emptyCellsCompressed.empty())
+		{
+			UASSERT(_emptyCellsCompressed.type() == CV_8UC1);
+			ctEmptyCells.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
 		ctUserData.join();
 		ctGroundCells.join();
 		ctObstacleCells.join();
+		ctEmptyCells.join();
 
 		if(imageRaw && imageRaw->empty())
 		{
@@ -763,6 +807,10 @@ void SensorData::uncompressDataConst(
 		{
 			*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
 		}
+		if(emptyCellsRaw && emptyCellsRaw->empty())
+		{
+			*emptyCellsRaw = ctEmptyCells.getUncompressedData();
+		}
 	}
 }
 
@@ -789,6 +837,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
 			_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
 			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
+			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
+			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
 			_keypoints.size() * sizeof(float) * 7 +
 			_keypoints3D.size() * sizeof(float)*3 +
 			_descriptors.total()*_descriptors.elemSize();

corelib/src/resources/DatabaseSchema.sql.in

@@ -39,6 +39,7 @@ CREATE TABLE Data (
 	
 	ground_cells BLOB,        -- compressed data (occupancy grid)
 	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
 	cell_size FLOAT,
 	view_point_x FLOAT,
 	view_point_y FLOAT,
@@ -114,6 +115,9 @@ CREATE TABLE Admin (
 	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
 	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
 	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
 
 	time_enter DATE
 );

corelib/src/util3d.cpp

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

corelib/src/util3d_mapping.cpp

@@ -48,8 +48,8 @@ namespace util3d
 
 void occupancy2DFromLaserScan(
 		const cv::Mat & scan,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
@@ -59,8 +59,8 @@ void occupancy2DFromLaserScan(
 			scan,
 			cv::Mat(),
 			viewpoint,
-			ground,
-			obstacles,
+			empty,
+			occupied,
 			cellSize,
 			unknownSpaceFilled,
 			scanMaxRange);
@@ -69,21 +69,21 @@ void occupancy2DFromLaserScan(
 void occupancy2DFromLaserScan(
 		const cv::Mat & scan,
 		const cv::Point3f & viewpoint,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
 {
-	occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, ground, obstacles, cellSize, unknownSpaceFilled, scanMaxRange);
+	occupancy2DFromLaserScan(scan, cv::Mat(), viewpoint, empty, occupied, cellSize, unknownSpaceFilled, scanMaxRange);
 }
 
 void occupancy2DFromLaserScan(
 		const cv::Mat & scanHit,
 		const cv::Mat & scanNoHit,
 		const cv::Point3f & viewpoint,
-		cv::Mat & ground,
-		cv::Mat & obstacles,
+		cv::Mat & empty,
+		cv::Mat & occupied,
 		float cellSize,
 		bool unknownSpaceFilled,
 		float scanMaxRange)
@@ -105,27 +105,27 @@ void occupancy2DFromLaserScan(
 	float xMin, yMin;
 	cv::Mat map8S = create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
 
-	// find ground cells
-	std::list<int> groundIndices;
+	// find empty cells
+	std::list<int> emptyIndices;
 	for(unsigned int i=0; i< map8S.total(); ++i)
 	{
 		if(map8S.data[i] == 0)
 		{
-			groundIndices.push_back(i);
+			emptyIndices.push_back(i);
 		}
 	}
 
 	// Convert to position matrices, get points to each center of the cells
-	ground = cv::Mat();
-	if(groundIndices.size())
+	empty = cv::Mat();
+	if(emptyIndices.size())
 	{
-		ground = cv::Mat(1, (int)groundIndices.size(), CV_32FC2);
+		empty = cv::Mat(1, (int)emptyIndices.size(), CV_32FC2);
 		int i=0;
-		for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
+		for(std::list<int>::iterator iter=emptyIndices.begin();iter!=emptyIndices.end(); ++iter)
 		{
 			int y = *iter / map8S.cols;
 			int x = *iter - y*map8S.cols;
-			cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
+			cv::Vec2f * ptr = empty.ptr<cv::Vec2f>();
 			ptr[i][0] = (float(x))*cellSize + xMin;
 			ptr[i][1] = (float(y))*cellSize + yMin;
 			++i;
@@ -133,7 +133,7 @@ void occupancy2DFromLaserScan(
 	}
 
 	// copy directly obstacles precise positions
-	obstacles = scanHit.clone();
+	occupied = scanHit.clone();
 }
 
 /**

guilib/include/rtabmap/gui/CloudViewer.h

@@ -142,7 +142,7 @@ public:
 			const cv::Mat & texture,
 			const Transform & pose = Transform::getIdentity());
 
-	bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0);
+	bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0, bool volumeRepresentation = true);
 	void removeOctomap();
 
 	// Only one texture per mesh is supported!

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -196,9 +196,9 @@ private:
 	std::multimap<int, rtabmap::Link> linksRefined_;
 	std::multimap<int, rtabmap::Link> linksAdded_;
 	std::multimap<int, rtabmap::Link> linksRemoved_;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles>
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps_; // < <ground, obstacles>, empty>
 	std::map<int, std::pair<float, cv::Point3f> > localMapsInfo_; // <cell size, viewpoint>
-	std::map<int, std::pair<cv::Mat, cv::Mat> > generatedLocalMaps_; // <ground, obstacles>
+	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > generatedLocalMaps_; // < <ground, obstacles>, empty>
 	std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
 	std::map<int, cv::Mat> modifiedDepthImages_;
 	OctoMap * octomap_;

guilib/include/rtabmap/gui/MainWindow.h

@@ -255,7 +255,7 @@ private:
 	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
-	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth, double stamp = 0.0, int refId = -1);
+	Transform alignPosesToGroundTruth(const std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
 	void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
 	void setupMainLayout(bool vertical);
 	void updateSelectSourceMenu();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -173,10 +173,9 @@ public:
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;
-	bool isOctomapCubeRendering() const;
+	int getOctomapRenderingType() const;
 	bool isOctomap2dGrid() const;
 	int getOctomapTreeDepth() const;
-	bool isOctomapFullUpdate() const;
 	double getOctomapOccupancyThr() const;
 	int getOctomapPointSize() const;
 	int getCloudDecimation(int index) const;   // 0=map, 1=odom
@@ -205,7 +204,6 @@ public:
 	double getSubtractFilteringAngle() const;
 
 	bool getGridMapShown() const;
-	double getGridMapResolution() const;;
 	bool isGridMapFrom3DCloud() const;
 	bool projMapFrame() const;
 	double projMaxGroundAngle() const;
@@ -305,7 +303,6 @@ private slots:
 	void updateKpROI();
 	void updateStereoDisparityVisibility();
 	void useOdomFeatures();
-	void useGridProjRayTracing();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
 	void changeOdometryORBSLAM2Vocabulary();

guilib/src/CloudViewer.cpp

@@ -52,6 +52,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <vtkCubeSource.h>
 #include <vtkGlyph3D.h>
 #include <vtkGlyph3DMapper.h>
+#include <vtkSmartVolumeMapper.h>
+#include <vtkVolumeProperty.h>
+#include <vtkColorTransferFunction.h>
+#include <vtkPiecewiseFunction.h>
+#include <vtkImageData.h>
 #include <vtkLookupTable.h>
 #include <vtkTextureUnitManager.h>
 #include <vtkJPEGReader.h>
@@ -792,7 +797,7 @@ bool CloudViewer::addCloudTextureMesh(
 	return false;
 }
 
-bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
+bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool volumeRepresentation)
 {
 	UDEBUG("");
 #ifdef RTABMAP_OCTOMAP
@@ -811,78 +816,190 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
 		treeDepth = octomap->octree()->getTreeDepth();
 	}
 
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
-	if(obstacles->size())
+	//get the renderer of the visualizer object
+	vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
+
+	if(!volumeRepresentation)
 	{
-		//get the renderer of the visualizer object
-		vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
-
-		if(_octomapActor)
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false);
+		if(obstacles->size())
 		{
-			renderer->RemoveActor(_octomapActor);
-			_octomapActor = 0;
+			if(_octomapActor)
+			{
+				renderer->RemoveActor(_octomapActor);
+				_octomapActor = 0;
+			}
+
+			//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
+			//colors->SetName("colors");
+			//colors->SetNumberOfComponents(3);
+			vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
+			colors->SetName("colors");
+			colors->SetNumberOfValues(obstacles->size());
+
+			vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
+			lut->SetNumberOfTableValues(obstacles->size());
+			lut->Build();
+
+			// Create points
+			vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
+			points->SetNumberOfPoints(obstacles->size());
+			double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
+			for (unsigned int i = 0; i < obstacles->size(); i++)
+			{
+				points->InsertPoint(i,
+						cloud->at(obstacles->at(i)).x,
+						cloud->at(obstacles->at(i)).y,
+						cloud->at(obstacles->at(i)).z);
+				colors->InsertValue(i,i);
+
+				lut->SetTableValue(i,
+						double(cloud->at(obstacles->at(i)).r) / 255.0,
+						double(cloud->at(obstacles->at(i)).g) / 255.0,
+						double(cloud->at(obstacles->at(i)).b) / 255.0);
+			}
+
+			// Combine into a polydata
+			vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
+			polydata->SetPoints(points);
+			polydata->GetPointData()->SetScalars(colors);
+
+			// Create anything you want here, we will use a cube for the demo.
+			vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
+			cubeSource->SetBounds(-s, s, -s, s, -s, s);
+
+			vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
+			mapper->SetSourceConnection(cubeSource->GetOutputPort());
+	#if VTK_MAJOR_VERSION <= 5
+			mapper->SetInputConnection(polydata->GetProducerPort());
+	#else
+			mapper->SetInputData(polydata);
+	#endif
+			mapper->SetScalarRange(0, obstacles->size() - 1);
+			mapper->SetLookupTable(lut);
+			mapper->ScalingOff();
+			mapper->Update();
+
+			vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
+			octomapActor->SetMapper(mapper);
+
+			octomapActor->GetProperty()->SetRepresentationToSurface();
+			octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+			octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+
+			renderer->AddActor(octomapActor);
+			_octomapActor = octomapActor.GetPointer();
+			return true;
 		}
-
-		//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
-		//colors->SetName("colors");
-		//colors->SetNumberOfComponents(3);
-		vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
-		colors->SetName("colors");
-		colors->SetNumberOfValues(obstacles->size());
-
-		vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
-		lut->SetNumberOfTableValues(obstacles->size());
-		lut->Build();
-
-		// Create points
-		vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
-		double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
-		for (unsigned int i = 0; i < obstacles->size(); i++)
+	}
+	else
+	{
+		if(octomap->octree()->size())
 		{
-			points->InsertNextPoint(
-					cloud->at(obstacles->at(i)).x,
-					cloud->at(obstacles->at(i)).y,
-					cloud->at(obstacles->at(i)).z);
-			colors->InsertValue(i,i);
+			if(_octomapActor)
+			{
+				renderer->RemoveActor(_octomapActor);
+				_octomapActor = 0;
+			}
+			// Create an image data
+			vtkSmartPointer<vtkImageData> imageData =
+					vtkSmartPointer<vtkImageData>::New();
 
-			lut->SetTableValue(i,
-					double(cloud->at(obstacles->at(i)).r) / 255.0,
-					double(cloud->at(obstacles->at(i)).g) / 255.0,
-					double(cloud->at(obstacles->at(i)).b) / 255.0);
-		}
+			double sizeX, sizeY, sizeZ;
+			double minX, minY, minZ;
+			double maxX, maxY, maxZ;
+			octomap->getGridMin(minX, minY, minZ);
+			octomap->getGridMax(maxX, maxY, maxZ);
+			sizeX = maxX-minX;
+			sizeY = maxY-minY;
+			sizeZ = maxZ-minZ;
+			double cellSize = octomap->octree()->getNodeSize(treeDepth);
 
-		// Combine into a polydata
-		vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
-		polydata->SetPoints(points);
-		polydata->GetPointData()->SetScalars(colors);
-
-		// Create anything you want here, we will use a cube for the demo.
-		vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
-		cubeSource->SetBounds(-s, s, -s, s, -s, s);
-
-		vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
-		mapper->SetSourceConnection(cubeSource->GetOutputPort());
+			UTimer t;
+			// Specify the size of the image data
+			imageData->SetExtent(0, int(sizeX/cellSize+0.5), 0, int(sizeY/cellSize+0.5), 0, int(sizeZ/cellSize+0.5)); // 3D image
 #if VTK_MAJOR_VERSION <= 5
-		mapper->SetInputConnection(polydata->GetProducerPort());
+			imageData->SetNumberOfScalarComponents(4);
+			imageData->SetScalarTypeToUnsignedChar();
 #else
-		mapper->SetInputData(polydata);
+			imageData->AllocateScalars(VTK_UNSIGNED_CHAR,4);
 #endif
-		mapper->SetScalarRange(0, obstacles->size() - 1);
-		mapper->SetLookupTable(lut);
-		mapper->ScalingOff();
-		mapper->Update();
 
-		vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
-		octomapActor->SetMapper(mapper);
+			int dims[3];
+			imageData->GetDimensions(dims);
 
-		octomapActor->GetProperty()->SetRepresentationToSurface();
-		octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
-		octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+			memset(imageData->GetScalarPointer(), 0, imageData->GetScalarSize()*imageData->GetNumberOfScalarComponents()*dims[0]*dims[1]*dims[2]);
 
-		renderer->AddActor(octomapActor);
-		_octomapActor = octomapActor.GetPointer();
+			for (RtabmapColorOcTree::iterator it = octomap->octree()->begin(treeDepth); it != octomap->octree()->end(); ++it)
+			{
+				if(octomap->octree()->isNodeOccupied(*it))
+				{
+					octomap::point3d pt = octomap->octree()->keyToCoord(it.getKey());
+					int x = (pt.x()-minX) / cellSize;
+					int y = (pt.y()-minY) / cellSize;
+					int z = (pt.z()-minZ) / cellSize;
+					if(x>=0 && x<dims[0] && y>=0 && y<dims[1] && z>=0 && z<dims[2])
+					{
+						unsigned char* pixel = static_cast<unsigned char*>(imageData->GetScalarPointer(x,y,z));
+						if(octomap->octree()->getTreeDepth() == it.getDepth() && it->isColorSet())
+						{
+							pixel[0] = it->getColor().r;
+							pixel[1] = it->getColor().g;
+							pixel[2] = it->getColor().b;
+						}
+						else
+						{
+							// Gradiant color on z axis
+							float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
+							float r,g,b;
+							OctoMap::HSVtoRGB(&r, &g, &b, H, 1, 1);
+							pixel[0] = r*255.0f;
+							pixel[1] = g*255.0f;
+							pixel[2] = b*255.0f;
+						}
+						pixel[3] = 255;
+					}
+				}
+			}
+			vtkSmartPointer<vtkSmartVolumeMapper> volumeMapper =
+					vtkSmartPointer<vtkSmartVolumeMapper>::New();
+			volumeMapper->SetBlendModeToComposite(); // composite first
+#if VTK_MAJOR_VERSION <= 5
+			volumeMapper->SetInputConnection(imageData->GetProducerPort());
+#else
+			volumeMapper->SetInputData(imageData);
+#endif
+			vtkSmartPointer<vtkVolumeProperty> volumeProperty =
+					vtkSmartPointer<vtkVolumeProperty>::New();
+			volumeProperty->ShadeOff();
+			volumeProperty->IndependentComponentsOff();
 
-		return true;
+			vtkSmartPointer<vtkPiecewiseFunction> compositeOpacity =
+					vtkSmartPointer<vtkPiecewiseFunction>::New();
+			compositeOpacity->AddPoint(0.0,0.0);
+			compositeOpacity->AddPoint(255.0,1.0);
+			volumeProperty->SetScalarOpacity(0, compositeOpacity); // composite first.
+
+			vtkSmartPointer<vtkVolume> volume =
+					vtkSmartPointer<vtkVolume>::New();
+			volume->SetMapper(volumeMapper);
+			volume->SetProperty(volumeProperty);
+			volume->SetScale(cellSize);
+			volume->SetPosition(minX, minY, minZ);
+			renderer->AddViewProp(volume);
+
+			// 3D texture mode. For coverage.
+#if !defined(VTK_LEGACY_REMOVE) && !defined(VTK_OPENGL2)
+			volumeMapper->SetRequestedRenderModeToRayCastAndTexture();
+#endif // VTK_LEGACY_REMOVE
+
+			// Software mode, for coverage. It also makes sure we will get the same
+			// regression image on all platforms.
+			volumeMapper->SetRequestedRenderModeToRayCast();
+			_octomapActor = volume.GetPointer();
+
+			return true;
+		}
 	}
 #endif
 	return false;
@@ -1913,6 +2030,12 @@ void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCamera
 {
 	std::vector<CameraModel> models;
 	models.push_back(model.left());
+	CameraModel right = model.right();
+	if(!model.left().localTransform().isNull())
+	{
+		right.setLocalTransform(model.left().localTransform() * Transform(model.baseline(), 0, 0, 0, 0, 0));
+	}
+	models.push_back(right);
 	updateCameraFrustums(pose, models);
 }
 

guilib/src/DatabaseViewer.cpp

@@ -188,6 +188,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
 	uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
 	uInsert(parameters, Parameters::getDefaultParameters("Grid"));
+	uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
 	parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
 	ui_->parameters_toolbox->setupUi(parameters);
@@ -196,12 +197,12 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	this->readSettings();
 
 	setupMainLayout(ui_->actionVertical_Layout->isChecked());
-	ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+	ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
-	ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+	ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 	ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
-	ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+	ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 
 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
@@ -321,10 +322,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
-	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
-	connect(ui_->checkBox_grid_cubes, SIGNAL(stateChanged(int)), this, SLOT(updateOctomapView()));
+	connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateOctomapView()));
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
@@ -334,8 +334,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 
-	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-
 	ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
 	ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
 	ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
@@ -355,10 +353,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
 	// Graph view
-	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -483,11 +479,9 @@ void DatabaseViewer::readSettings()
 	settings.endGroup();
 
 	settings.beginGroup("grid");
-	ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
 	ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
 	ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
 	ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
-	ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -564,11 +558,9 @@ void DatabaseViewer::writeSettings()
 
 	// save Grid settings
 	settings.beginGroup("grid");
-	settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
 	settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
 	settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
 	settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
-	settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -640,11 +632,9 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
 
-	ui_->doubleSpinBox_gridCellSize->setValue(0.05);
 	ui_->groupBox_posefiltering->setChecked(false);
 	ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
 	ui_->doubleSpinBox_posefilteringAngle->setValue(30);
-	ui_->checkBox_gridErode->setChecked(false);
 	ui_->checkBox_octomap->setChecked(false);
 
 	ui_->checkBox_mesh_quad->setChecked(true);
@@ -845,14 +835,15 @@ bool DatabaseViewer::closeDatabase()
 			{
 				// Rejected links
 				UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
-				std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mapIter = generatedLocalMaps_.begin();
+				std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
 				std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
 				for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
 				{
 					UASSERT(mapIter->first == infoIter->first);
 					dbDriver_->updateOccupancyGrid(
 							mapIter->first,
-							mapIter->second.first,
+							mapIter->second.first.first,
+							mapIter->second.first.second,
 							mapIter->second.second,
 							infoIter->second.first,
 							infoIter->second.second);
@@ -2308,8 +2299,9 @@ void DatabaseViewer::regenerateLocalMaps()
 	plotCells->setWindowTitle("Occupancy Cells");
 	plotCells->setAttribute(Qt::WA_DeleteOnClose);
 	UPlotCurve * totalCurve = plotCells->addCurve("Total");
-	UPlotCurve * groundCurve = plotCells->addCurve("Empty");
-	UPlotCurve * obstaclesCurve = plotCells->addCurve("Occupied");
+	UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
+	UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
+	UPlotCurve * groundCurve = plotCells->addCurve("Ground");
 	plotCells->show();
 
 	double decompressionTime = 0;
@@ -2334,18 +2326,63 @@ void DatabaseViewer::regenerateLocalMaps()
 		{
 			Signature s = data;
 			s.setPose(odomPose);
-			cv::Mat ground, obstacles;
+			cv::Mat ground, obstacles, empty;
 			cv::Point3f viewpoint;
 			timer.ticks();
-			grid.createLocalMap(s, ground, obstacles, viewpoint);
+
+			if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
+				*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
+
+				if(cloud->size())
+				{
+					// update viewpoint
+					if(s.sensorData().cameraModels().size())
+					{
+						// average of all local transforms
+						float sum = 0;
+						for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
+						{
+							const Transform & t = s.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
+					{
+						const Transform & t = s.sensorData().stereoCameraModel().localTransform();
+						viewpoint = cv::Point3f(t.x(), t.y(), t.z());
+					}
+
+					grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
+				}
+			}
+			else
+			{
+				grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
+			}
+
 			gridCreationTime = timer.ticks()*1000.0;
-			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
+			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty)));
 			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
 			msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
 
-			totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols);
-			groundCurve->addValue(ids_.at(i), ground.cols);
+			totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
+			emptyCurve->addValue(ids_.at(i), empty.cols);
 			obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
+			groundCurve->addValue(ids_.at(i), ground.cols);
 		}
 
 		progressDialog.appendText(msg);
@@ -2412,10 +2449,55 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 		{
 			Signature s = data;
 			s.setPose(odomPose);
-			cv::Mat ground, obstacles;
+			cv::Mat ground, obstacles, empty;
 			cv::Point3f viewpoint;
-			grid.createLocalMap(s, ground, obstacles, viewpoint);
-			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
+
+			if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
+			{
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
+				*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
+
+				if(cloud->size())
+				{
+					// update viewpoint
+					if(s.sensorData().cameraModels().size())
+					{
+						// average of all local transforms
+						float sum = 0;
+						for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
+						{
+							const Transform & t = s.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
+					{
+						const Transform & t = s.sensorData().stereoCameraModel().localTransform();
+						viewpoint = cv::Point3f(t.x(), t.y(), t.z());
+					}
+
+					grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
+				}
+			}
+			else
+			{
+				grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
+			}
+
+
+			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
 			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
 			msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
 		}
@@ -2915,6 +2997,7 @@ void DatabaseViewer::update(int value,
 					cloudViewer_->removeCloud("map");
 					cloudViewer_->removeCloud("ground");
 					cloudViewer_->removeCloud("obstacles");
+					cloudViewer_->removeCloud("empty_cells");
 					cloudViewer_->removeCloud("words");
 					cloudViewer_->removeOctomap();
 					if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
@@ -3022,7 +3105,6 @@ void DatabaseViewer::update(int value,
 									}
 
 									cloudViewer_->addCloud("cloud", cloud, pose);
-									cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
 								}
 							}
 						}
@@ -3031,7 +3113,14 @@ void DatabaseViewer::update(int value,
 					//frustums
 					if(cloudViewer_->isFrustumShown())
 					{
-						cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
+						if(data.cameraModels().size())
+						{
+							cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
+						}
+						else
+						{
+							cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
+						}
 					}
 
 					//words
@@ -3102,7 +3191,7 @@ void DatabaseViewer::update(int value,
 					//add occupancy grid
 					if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
 					{
-						std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
+						std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
 						std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
 						if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
 						{
@@ -3111,7 +3200,7 @@ void DatabaseViewer::update(int value,
 						}
 						else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
 						{
-							localMaps.insert(std::make_pair(data.id(), std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw())));
+							localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
 							localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
 						}
 						if(!localMaps.empty())
@@ -3122,14 +3211,15 @@ void DatabaseViewer::update(int value,
 #ifdef RTABMAP_OCTOMAP
 							OctoMap * octomap = 0;
 							if(ui_->checkBox_octomap->isChecked() &&
-								(!localMaps.begin()->second.first.empty() || !localMaps.begin()->second.second.empty()) &&
-								(localMaps.begin()->second.first.empty() || localMaps.begin()->second.first.channels() > 2) &&
+								(!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
+								(localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
+								(localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
 								(localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
 								localMapsInfo.begin()->second.first > 0.0f)
 							{
 								//create local octomap
 								octomap = new OctoMap(localMapsInfo.begin()->second.first);
-								octomap->addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
+								octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
 								octomap->update(poses);
 							}
 #endif
@@ -3138,7 +3228,9 @@ void DatabaseViewer::update(int value,
 							{
 								float xMin=0.0f, yMin=0.0f;
 								cv::Mat map8S;
-								float gridCellSize = ui_->doubleSpinBox_gridCellSize->value();
+								ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+								float gridCellSize = Parameters::defaultGridCellSize();
+								Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
 #ifdef RTABMAP_OCTOMAP
 								if(octomap)
 								{
@@ -3147,15 +3239,11 @@ void DatabaseViewer::update(int value,
 								else
 #endif
 								{
-									map8S = util3d::create2DMapFromOccupancyLocalMaps(
-																poses,
-																localMaps,
-																ui_->doubleSpinBox_gridCellSize->value(),
-																xMin, yMin);
-									//OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
-									//grid.addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second);
-									//grid.update(poses);
-									//map8S = grid.getMap(xMin, yMin);
+									OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+									grid.setCellSize(gridCellSize);
+									grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
+									grid.update(poses);
+									map8S = grid.getMap(xMin, yMin);
 								}
 								if(!map8S.empty())
 								{
@@ -3169,27 +3257,34 @@ void DatabaseViewer::update(int value,
 #ifdef RTABMAP_OCTOMAP
 								if(octomap)
 								{
-									if(!ui_->checkBox_grid_cubes->isChecked())
+									if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
 									{
 										pcl::IndicesPtr obstacles(new std::vector<int>);
 										pcl::IndicesPtr empty(new std::vector<int>);
-										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get());
+										pcl::IndicesPtr ground(new std::vector<int>);
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
 										pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 										pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
 										cloudViewer_->addCloud("obstacles", obstaclesCloud);
 										cloudViewer_->setCloudPointSize("obstacles", 5);
+
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+										pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+										cloudViewer_->addCloud("ground", groundCloud);
+										cloudViewer_->setCloudPointSize("ground", 5);
+
 										if(ui_->checkBox_grid_empty->isChecked())
 										{
 											pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
 											pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
-											cloudViewer_->addCloud("ground", emptyCloud, Transform::getIdentity(), Qt::white);
-											cloudViewer_->setCloudOpacity("ground", 0.5);
-											cloudViewer_->setCloudPointSize("ground", 5);
+											cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), Qt::white);
+											cloudViewer_->setCloudOpacity("empty_cells", 0.5);
+											cloudViewer_->setCloudPointSize("empty_cells", 1);
 										}
 									}
 									else
 									{
-										cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value());
+										cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
 									}
 								}
 								else
@@ -3197,15 +3292,25 @@ void DatabaseViewer::update(int value,
 								{
 									// occupancy cloud
 									cloudViewer_->addCloud("ground",
-											util3d::laserScanToPointCloud(localMaps.begin()->second.first),
+											util3d::laserScanToPointCloud(localMaps.begin()->second.first.first),
 											pose,
 											Qt::green);
 									cloudViewer_->addCloud("obstacles",
-											util3d::laserScanToPointCloud(localMaps.begin()->second.second),
+											util3d::laserScanToPointCloud(localMaps.begin()->second.first.second),
 											pose,
 											Qt::red);
 									cloudViewer_->setCloudPointSize("ground", 5);
 									cloudViewer_->setCloudPointSize("obstacles", 5);
+
+									if(ui_->checkBox_grid_empty->isChecked())
+									{
+										cloudViewer_->addCloud("empty_cells",
+												util3d::laserScanToPointCloud(localMaps.begin()->second.second),
+												pose,
+												Qt::white);
+										cloudViewer_->setCloudPointSize("empty_cells", 1);
+										cloudViewer_->setCloudOpacity("empty_cells", 0.5);
+									}
 								}
 							}
 #ifdef RTABMAP_OCTOMAP
@@ -4347,7 +4452,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					ui_->doubleSpinBox_posefilteringRadius->value(),
 					ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
 		}
-		std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
+		std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
 		std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
 #ifdef RTABMAP_OCTOMAP
 		if(octomap_)
@@ -4370,7 +4475,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				}
 				else if(localMaps_.find(ids[i]) != localMaps_.end())
 				{
-					if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.second.empty())
+					if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
 					{
 						localMaps.insert(*localMaps_.find(ids.at(i)));
 						localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
@@ -4380,19 +4485,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				{
 					SensorData data;
 					dbDriver_->getNodeData(ids.at(i), data);
-					cv::Mat ground, obstacles;
-					data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
-					localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
+					cv::Mat ground, obstacles, empty;
+					data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
+					localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
 					localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
 					if(!ground.empty() || !obstacles.empty())
 					{
-						localMaps.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
+						localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
 						localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
 					}
 				}
 			}
 			//cleanup
-			for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
+			for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
 			{
 				if(graphFiltered.find(iter->first) == graphFiltered.end())
 				{
@@ -4407,6 +4512,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
 		}
 
+		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+		float cellSize = Parameters::defaultGridCellSize();
+		Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
+
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
 		ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
 		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
@@ -4421,11 +4530,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 #ifdef RTABMAP_OCTOMAP
 			if(ui_->checkBox_octomap->isChecked())
 			{
-				octomap_ = new OctoMap(ui_->doubleSpinBox_gridCellSize->value());
+				octomap_ = new OctoMap(cellSize);
 				bool updateAborted = false;
-				for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
+				for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
 				{
-					if(iter->second.first.channels() == 2 || iter->second.second.channels() == 2)
+					if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
 					{
 						QMessageBox::warning(this, tr(""),
 								tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
@@ -4434,7 +4543,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 						updateAborted = true;
 						break;
 					}
-					octomap_->addToCache(iter->first, iter->second.first, iter->second.second, localMapsInfo.at(iter->first).second);
+					octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
 				}
 				if(!updateAborted)
 				{
@@ -4447,19 +4556,31 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
 			   (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
 			{
+				bool eroded = Parameters::defaultGridGlobalEroded();
+				Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
 				float xMin, yMin;
-				float cell = ui_->doubleSpinBox_gridCellSize->value();
 				cv::Mat map;
 
 #ifdef RTABMAP_OCTOMAP
 				if(ui_->checkBox_octomap->isChecked())
 				{
-					map = octomap_->createProjectionMap(xMin, yMin, cell, 0, ui_->spinBox_grid_depth->value());
+					map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
 				}
 				else
 #endif
 				{
-					map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
+					if(eroded)
+					{
+						uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
+					}
+					OccupancyGrid grid(parameters);
+					grid.setCellSize(cellSize);
+					for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
+					{
+						grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
+					}
+					grid.update(graphFiltered);
+					map = grid.getMap(xMin, yMin);
 				}
 
 				ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
@@ -4469,11 +4590,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
 					if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
 					{
-						ui_->graphViewer->updateMap(map8U, cell, xMin, yMin);
+						ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
 					}
 					if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
 					{
-						occupancyGridViewer_->addOccupancyGridMap(map8U, cell, xMin, yMin, 1.0f);
+						occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
 						occupancyGridViewer_->update();
 					}
 				}
@@ -4492,42 +4613,58 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				{
 					pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
 					pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCellsXYZ(new pcl::PointCloud<pcl::PointXYZ>);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-					for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
+					for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
 					{
 						Transform pose = graphFiltered.at(iter->first);
 						float x,y,z,roll,pitch,yaw;
 						pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 						Transform pose2d(x,y, 0, 0, 0, yaw);
-						if(!iter->second.first.empty())
+						if(!iter->second.first.first.empty())
 						{
-							if(iter->second.first.channels() == 4)
+							if(iter->second.first.first.channels() == 4)
 							{
-								*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first, pose);
+								*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.first, pose);
 							}
 							else
 							{
-								*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first, iter->second.first.channels()==2?pose2d:pose);
+								*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first.first, iter->second.first.first.channels()==2?pose2d:pose);
 							}
 						}
-						if(!iter->second.second.empty())
+						if(!iter->second.first.second.empty())
 						{
-							if(iter->second.second.channels() == 4)
+							if(iter->second.first.second.channels() == 4)
 							{
-								*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
+								*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.second, pose);
 							}
 							else
 							{
-								*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.second, iter->second.second.channels()==2?pose2d:pose);
+								*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.first.second, iter->second.first.second.channels()==2?pose2d:pose);
+							}
+						}
+						if(ui_->checkBox_grid_empty->isChecked())
+						{
+							if(!iter->second.second.empty())
+							{
+								if(iter->second.second.channels() == 4)
+								{
+									*emptyCellsRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
+								}
+								else
+								{
+									*emptyCellsXYZ += *util3d::laserScanToPointCloud(iter->second.second, iter->second.second.channels()==2?pose2d:pose);
+								}
 							}
 						}
 					}
 					// occupancy cloud
 					if(groundRGB->size())
 					{
-						groundRGB = util3d::voxelize(groundRGB, ui_->doubleSpinBox_gridCellSize->value());
+						groundRGB = util3d::voxelize(groundRGB, cellSize);
 						occupancyGridViewer_->addCloud("groundRGB",
 								groundRGB,
 								Transform::getIdentity(),
@@ -4536,7 +4673,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					}
 					if(groundXYZ->size())
 					{
-						groundXYZ = util3d::voxelize(groundXYZ, ui_->doubleSpinBox_gridCellSize->value());
+						groundXYZ = util3d::voxelize(groundXYZ, cellSize);
 						occupancyGridViewer_->addCloud("groundXYZ",
 								groundXYZ,
 								Transform::getIdentity(),
@@ -4545,7 +4682,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					}
 					if(obstaclesRGB->size())
 					{
-						obstaclesRGB = util3d::voxelize(obstaclesRGB, ui_->doubleSpinBox_gridCellSize->value());
+						obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
 						occupancyGridViewer_->addCloud("obstaclesRGB",
 								obstaclesRGB,
 								Transform::getIdentity(),
@@ -4554,13 +4691,33 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					}
 					if(obstaclesXYZ->size())
 					{
-						obstaclesXYZ = util3d::voxelize(obstaclesXYZ, ui_->doubleSpinBox_gridCellSize->value());
+						obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
 						occupancyGridViewer_->addCloud("obstaclesXYZ",
 								obstaclesXYZ,
 								Transform::getIdentity(),
 								Qt::red);
 						occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
 					}
+					if(emptyCellsRGB->size())
+					{
+						emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
+						occupancyGridViewer_->addCloud("emptyCellsRGB",
+								emptyCellsRGB,
+								Transform::getIdentity(),
+								Qt::white);
+						occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 1);
+						occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
+					}
+					if(emptyCellsXYZ->size())
+					{
+						emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
+						occupancyGridViewer_->addCloud("emptyCellsXYZ",
+								emptyCellsXYZ,
+								Transform::getIdentity(),
+								Qt::white);
+						occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 1);
+						occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
+					}
 					occupancyGridViewer_->update();
 				}
 			}
@@ -4876,12 +5033,12 @@ void DatabaseViewer::updateGrid()
 	}
 	else
 	{
-		ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
-		ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
-		ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 
 		update3dView();
 		updateGraphView();
@@ -4891,12 +5048,12 @@ void DatabaseViewer::updateGrid()
 void DatabaseViewer::updateOctomapView()
 {
 #ifdef RTABMAP_OCTOMAP
-		ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
-		ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
-		ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
 
 		if(ui_->checkBox_octomap->isChecked())
 		{
@@ -4905,19 +5062,26 @@ void DatabaseViewer::updateOctomapView()
 				occupancyGridViewer_->removeOctomap();
 				occupancyGridViewer_->removeCloud("octomap_obstacles");
 				occupancyGridViewer_->removeCloud("octomap_empty");
-				if(ui_->checkBox_grid_cubes->isChecked())
+				if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
 				{
-					occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value());
+					occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
 				}
 				else
 				{
 					pcl::IndicesPtr obstacles(new std::vector<int>);
 					pcl::IndicesPtr empty(new std::vector<int>);
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get());
+					pcl::IndicesPtr ground(new std::vector<int>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
-					occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud);
+					occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
 					occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
+
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+					pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+					occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
+					occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
+
 					if(ui_->checkBox_grid_empty->isChecked())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);

guilib/src/MainWindow.cpp

@@ -254,9 +254,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
 #ifdef RTABMAP_OCTOMAP
 	_octomap = new OctoMap(
-			_preferencesDialog->getGridMapResolution(),
+			_occupancyGrid->getCellSize(),
 			_preferencesDialog->getOctomapOccupancyThr(),
-			_preferencesDialog->isOctomapFullUpdate());
+			_occupancyGrid->isFullUpdate(),
+			_occupancyGrid->getUpdateError());
 #endif
 
 	// Timer
@@ -1860,7 +1861,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			}
 
 			std::map<int, Transform> poses = stat.poses();
-			Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth, stat.stamp(), stat.refImageId());
 			UDEBUG("time= %d ms", time.restart());
 
 			if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
@@ -1913,7 +1913,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			_odometryReceived = false;
 
-			_odometryCorrection = groundTruthOffset * stat.mapCorrection();
+			_odometryCorrection = stat.mapCorrection();
 
 			UDEBUG("time= %d ms", time.restart());
 
@@ -2243,10 +2243,11 @@ void MainWindow::updateMapCloud(
 				{
 					cv::Mat ground;
 					cv::Mat obstacles;
+					cv::Mat empty;
 
-					jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+					jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
 
-					_occupancyGrid->addToCache(iter->first, ground, obstacles);
+					_occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
 
 #ifdef RTABMAP_OCTOMAP
 					if(updateOctomap)
@@ -2255,7 +2256,7 @@ void MainWindow::updateMapCloud(
 						   (obstacles.empty() || obstacles.channels() > 2))
 						{
 							cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
-							_octomap->addToCache(iter->first, ground, obstacles, viewpoint);
+							_octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
 						}
 						else if(!ground.empty() || !obstacles.empty())
 						{
@@ -2370,6 +2371,13 @@ void MainWindow::updateMapCloud(
 			}
 		}
 	}
+
+	Transform mapToGt = Transform::getIdentity();
+	if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
+	{
+		mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
+	}
+
 	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
 	{
 		UTimer timerGraph;
@@ -2436,7 +2444,8 @@ void MainWindow::updateMapCloud(
 				{
 					kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
 				}
-				pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
+				Transform t = mapToGt*iter->second;
+				pcl::PointXYZ pt(t.x(), t.y(), t.z());
 				kter->second->push_back(pt);
 			}
 		}
@@ -2524,9 +2533,9 @@ void MainWindow::updateMapCloud(
 	{
 		UDEBUG("");
 		UTimer time;
-		if(_preferencesDialog->isOctomapCubeRendering())
+		if(_preferencesDialog->getOctomapRenderingType() > 0)
 		{
-			_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
+			_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
 		}
 		else
 		{
@@ -2551,13 +2560,25 @@ void MainWindow::updateMapCloud(
 	if(_ui->graphicsView_graphView->isVisible())
 	{
 		_ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
-		_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
+		if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
+		{
+			std::map<int, Transform> gtPoses = _currentGTPosesMap;
+			for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
+			{
+				iter->second = mapToGt * iter->second;
+			}
+			_ui->graphicsView_graphView->updateGTGraph(gtPoses);
+		}
+		else
+		{
+			_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
+		}
 	}
 	cv::Mat map8U;
 	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
 	{
 		float xMin, yMin;
-		float resolution = _preferencesDialog->getGridMapResolution();
+		float resolution = _occupancyGrid->getCellSize();
 		cv::Mat map8S;
 #ifdef RTABMAP_OCTOMAP
 		if(_preferencesDialog->isOctomap2dGrid())
@@ -3323,10 +3344,8 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
 }
 
 Transform MainWindow::alignPosesToGroundTruth(
-		std::map<int, Transform> & poses,
-		const std::map<int, Transform> & groundTruth,
-		double stamp,
-		int refId)
+		const std::map<int, Transform> & poses,
+		const std::map<int, Transform> & groundTruth)
 {
 	Transform t = Transform::getIdentity();
 	if(groundTruth.size() && poses.size())
@@ -3344,7 +3363,7 @@ Transform MainWindow::alignPosesToGroundTruth(
 		float rotational_min = 0.0f;
 		float rotational_max = 0.0f;
 
-		Transform gtToMap = graph::calcRMSE(
+		t = graph::calcRMSE(
 				groundTruth,
 				poses,
 				translational_rmse,
@@ -3360,16 +3379,6 @@ Transform MainWindow::alignPosesToGroundTruth(
 				rotational_min,
 				rotational_max);
 
-		if(_preferencesDialog->isGroundTruthAligned())
-		{
-			t = gtToMap;
-
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-			{
-				iter->second = gtToMap * iter->second;
-			}
-		}
-
 		// ground truth live statistics
 		UINFO("translational_rmse=%f", translational_rmse);
 		UINFO("translational_mean=%f", translational_mean);
@@ -3653,7 +3662,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 			_progressCanceled = false;
 			QApplication::processEvents();
 			std::map<int, Transform> poses = event.getPoses();
-			alignPosesToGroundTruth(poses, groundTruth);
 			this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
 			_progressDialog->appendText("Updating the 3D map cloud... done.");
 		}
@@ -4725,9 +4733,10 @@ void MainWindow::startDetection()
 	UASSERT(_octomap != 0);
 	delete _octomap;
 	_octomap = new OctoMap(
-			_preferencesDialog->getGridMapResolution(),
+			_occupancyGrid->getCellSize(),
 			_preferencesDialog->getOctomapOccupancyThr(),
-			_preferencesDialog->isOctomapFullUpdate());
+			_occupancyGrid->isFullUpdate(),
+			_occupancyGrid->getUpdateError());
 #endif
 
 	// clear odometry visual stuff
@@ -5577,7 +5586,6 @@ void MainWindow::postProcessing()
 	}
 
 	_progressDialog->appendText(tr("Updating map..."));
-	alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
 	this->updateMapCloud(
 			optimizedPoses,
 			std::multimap<int, Link>(_currentLinksMap),
@@ -6043,9 +6051,10 @@ void MainWindow::clearTheCache()
 	UASSERT(_octomap != 0);
 	delete _octomap;
 	_octomap = new OctoMap(
-			_preferencesDialog->getGridMapResolution(),
+			_occupancyGrid->getCellSize(),
 			_preferencesDialog->getOctomapOccupancyThr(),
-			_preferencesDialog->isOctomapFullUpdate());
+			_occupancyGrid->isFullUpdate(),
+			_occupancyGrid->getUpdateError());
 #endif
 	_occupancyGrid->clear();
 }

guilib/src/PreferencesDialog.cpp

@@ -453,7 +453,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->comboBox_octomap_renderingType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_pointSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_octomap_occupancyThr, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
@@ -915,8 +915,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
 	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
 	_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
-	_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridProjRayTracing().c_str());
-	connect(_ui->checkBox_grid_projRayTracing, SIGNAL(stateChanged(int)), this, SLOT(useGridProjRayTracing()));
+	_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridRayTracing().c_str());
 	_ui->doubleSpinBox_grid_footprintLength->setObjectName(Parameters::kGridFootprintLength().c_str());
 	_ui->doubleSpinBox_grid_footprintWidth->setObjectName(Parameters::kGridFootprintWidth().c_str());
 	_ui->doubleSpinBox_grid_footprintHeight->setObjectName(Parameters::kGridFootprintHeight().c_str());
@@ -938,6 +937,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());
 
 	_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str());
+	_ui->doubleSpinBox_grid_updateError->setObjectName(Parameters::kGridGlobalUpdateError().c_str());
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
 	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
 	_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
@@ -1466,7 +1466,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_octomap_treeDepth->setValue(16);
 		_ui->checkBox_octomap_2dgrid->setChecked(true);
 		_ui->checkBox_octomap_show3dMap->setChecked(true);
-		_ui->checkBox_octomap_cubeRendering->setChecked(false);
+		_ui->comboBox_octomap_renderingType->setCurrentIndex(0);
 		_ui->spinBox_octomap_pointSize->setValue(5);
 		_ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5);
 	}
@@ -1854,7 +1854,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
 	_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
 	_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
-	_ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool());
+	_ui->comboBox_octomap_renderingType->setCurrentIndex(settings.value("octomap_rendering_type", _ui->comboBox_octomap_renderingType->currentIndex()).toInt());
 	_ui->doubleSpinBox_octomap_occupancyThr->setValue(settings.value("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value()).toDouble());
 	_ui->spinBox_octomap_pointSize->setValue(settings.value("octomap_point_size", _ui->spinBox_octomap_pointSize->value()).toInt());
 
@@ -2257,7 +2257,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("octomap_depth",               _ui->spinBox_octomap_treeDepth->value());
 	settings.setValue("octomap_2dgrid",              _ui->checkBox_octomap_2dgrid->isChecked());
 	settings.setValue("octomap_3dmap",               _ui->checkBox_octomap_show3dMap->isChecked());
-	settings.setValue("octomap_cube",                _ui->checkBox_octomap_cubeRendering->isChecked());
+	settings.setValue("octomap_rendering_type",      _ui->comboBox_octomap_renderingType->currentIndex());
 	settings.setValue("octomap_occupancy_thr",       _ui->doubleSpinBox_octomap_occupancyThr->value());
 	settings.setValue("octomap_point_size",          _ui->spinBox_octomap_pointSize->value());
 
@@ -4082,22 +4082,6 @@ void PreferencesDialog::useOdomFeatures()
 	}
 }
 
-
-void PreferencesDialog::useGridProjRayTracing()
-{
-	if(this->isVisible() && _ui->checkBox_grid_projRayTracing->isChecked() && _ui->groupBox_grid_3d->isChecked())
-	{
-		int r = QMessageBox::question(this, tr("Using ray tracing for 2D projection..."),
-				tr("Currently the 3D occupancy grid parameter is checked, but 2D ray tracing "
-				  "only works with 2D occupancy grids. Do you want to uncheck 3D occupancy grid?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
-
-		if(r == QMessageBox::Yes)
-		{
-			_ui->groupBox_grid_3d->setChecked(false);
-		}
-	}
-}
-
 void PreferencesDialog::changeWorkingDirectory()
 {
 	QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());
@@ -4319,9 +4303,9 @@ bool PreferencesDialog::isOctomapShown() const
 #endif
 	return false;
 }
-bool PreferencesDialog::isOctomapCubeRendering() const
+int PreferencesDialog::getOctomapRenderingType() const
 {
-	return _ui->checkBox_octomap_cubeRendering->isChecked();
+	return _ui->comboBox_octomap_renderingType->currentIndex();
 }
 bool PreferencesDialog::isOctomap2dGrid() const
 {
@@ -4334,10 +4318,6 @@ int PreferencesDialog::getOctomapTreeDepth() const
 {
 	return _ui->spinBox_octomap_treeDepth->value();
 }
-bool PreferencesDialog::isOctomapFullUpdate() const
-{
-	return uStr2Bool(this->getParameter(Parameters::kGridGlobalFullUpdate()));
-}
 double PreferencesDialog::getOctomapOccupancyThr() const
 {
 	return _ui->doubleSpinBox_octomap_occupancyThr->value();
@@ -4538,10 +4518,6 @@ bool PreferencesDialog::getGridMapShown() const
 {
 	return _ui->checkBox_map_shown->isChecked();
 }
-double PreferencesDialog::getGridMapResolution() const
-{
-	return _ui->doubleSpinBox_grid_resolution->value();
-}
 bool PreferencesDialog::isGridMapFrom3DCloud() const
 {
 	return _ui->groupBox_grid_fromDepthImage->isChecked();

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>395</width>
+           <width>326</width>
            <height>242</height>
           </rect>
          </property>
@@ -253,7 +253,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>394</width>
+           <width>325</width>
            <height>242</height>
           </rect>
          </property>
@@ -1163,7 +1163,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1332,8 +1332,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>302</width>
-          <height>696</height>
+          <width>451</width>
+          <height>668</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1341,7 +1341,7 @@
         </attribute>
         <layout class="QVBoxLayout" name="verticalLayout_16">
          <item>
-          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
+          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
            <item row="7" column="1">
             <widget class="QLabel" name="label_51">
              <property name="text">
@@ -1374,7 +1374,7 @@
              </property>
             </widget>
            </item>
-           <item row="2" column="1">
+           <item row="1" column="1">
             <widget class="QLabel" name="label_53">
              <property name="text">
               <string>OctoMap</string>
@@ -1391,41 +1391,7 @@
              </property>
             </widget>
            </item>
-           <item row="1" column="1">
-            <widget class="QLabel" name="label_44">
-             <property name="text">
-              <string>Errode</string>
-             </property>
-            </widget>
-           </item>
            <item row="1" column="0">
-            <widget class="QCheckBox" name="checkBox_gridErode">
-             <property name="text">
-              <string/>
-             </property>
-            </widget>
-           </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_46">
-             <property name="text">
-              <string>Grid cell size</string>
-             </property>
-            </widget>
-           </item>
-           <item row="0" column="0">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_gridCellSize">
-             <property name="decimals">
-              <number>3</number>
-             </property>
-             <property name="minimum">
-              <double>0.001000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
-             </property>
-            </widget>
-           </item>
-           <item row="2" column="0">
             <widget class="QCheckBox" name="checkBox_octomap">
              <property name="text">
               <string/>
@@ -1455,26 +1421,6 @@
             </widget>
            </item>
            <item row="5" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_cubes">
-             <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="0">
-            <widget class="QCheckBox" name="checkBox_grid_empty">
-             <property name="text">
-              <string/>
-             </property>
-             <property name="checked">
-              <bool>false</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="6" column="0">
             <widget class="QSpinBox" name="spinBox_grid_depth">
              <property name="prefix">
               <string/>
@@ -1490,25 +1436,83 @@
              </property>
             </widget>
            </item>
-           <item row="4" column="1">
-            <widget class="QLabel" name="label_octomap_empty">
+           <item row="6" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
              <property name="text">
-              <string>OctoMap: Empty space</string>
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="1">
+            <widget class="QLabel" name="label_octomap_cubes">
+             <property name="text">
+              <string>OctoMap: Rendering type</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
              </property>
             </widget>
            </item>
            <item row="5" column="1">
-            <widget class="QLabel" name="label_octomap_cubes">
-             <property name="text">
-              <string>OctoMap: Cubes rendering</string>
-             </property>
-            </widget>
-           </item>
-           <item row="6" column="1">
             <widget class="QLabel" name="label_octomap_depth">
              <property name="text">
               <string>OctoMap: Tree depth</string>
              </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_empty">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="6" column="1">
+            <widget class="QLabel" name="label_55">
+             <property name="text">
+              <string>Local grid: regenerate from saved grid instead of sensors</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="1">
+            <widget class="QLabel" name="label_octomap_empty">
+             <property name="text">
+              <string>Global grid/OctoMap: show empty space</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="0">
+            <widget class="QComboBox" name="comboBox_octomap_rendering_type">
+             <item>
+              <property name="text">
+               <string>Point Cloud</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Cube</string>
+              </property>
+             </item>
+             <item>
+              <property name="text">
+               <string>Volume</string>
+              </property>
+             </item>
             </widget>
            </item>
           </layout>

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>678</width>
-            <height>2739</height>
+            <height>2778</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>16</number>
+                <number>3</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -259,7 +259,7 @@
                       <item row="7" column="1">
                        <widget class="QLabel" name="label_347">
                         <property name="text">
-                         <string>When a ground truth is provided, align the map with it.</string>
+                         <string>When a ground truth is provided, align it with the map.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -2107,20 +2107,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="2" column="0">
-                        <widget class="QCheckBox" name="checkBox_octomap_cubeRendering">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>false</bool>
-                         </property>
-                        </widget>
-                       </item>
                        <item row="2" column="1">
                         <widget class="QLabel" name="label_octomap_treeDepth_5">
                          <property name="text">
-                          <string>Cube rendering. Warning: this requires significant more GPU power.</string>
+                          <string>OctoMap rendering type. Cube rendering requires significant more GPU power.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -2179,6 +2169,28 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="2" column="0">
+                        <widget class="QComboBox" name="comboBox_octomap_renderingType">
+                         <property name="sizeAdjustPolicy">
+                          <enum>QComboBox::AdjustToContents</enum>
+                         </property>
+                         <item>
+                          <property name="text">
+                           <string>Point Cloud</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Cube</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Volume</string>
+                          </property>
+                         </item>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -9499,7 +9511,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          <item row="4" column="1">
                           <widget class="QLabel" name="label_363">
                            <property name="text">
-                            <string>2D ray tracing is done for each projected obstacle (when 3D is not checked below), filling unknown space between the sensor and obstacles.</string>
+                            <string>Ray tracing is done for each obstacle, filling unknown space between the sensor and obstacles. If RTAB-Map is not built with OctoMap and 3D is checked below, 3D ray tracing cannot be done.</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -9876,7 +9888,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="1">
+                      <item row="5" column="1">
                        <widget class="QLabel" name="label_224">
                         <property name="text">
                          <string>Erode obstacle cells.</string>
@@ -9889,7 +9901,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
+                      <item row="4" column="1">
                        <widget class="QLabel" name="label_319">
                         <property name="text">
                          <string>Footprint radius used to clear all obstacles under the graph.</string>
@@ -9902,7 +9914,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
+                      <item row="5" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_erode">
                         <property name="text">
                          <string/>
@@ -9912,7 +9924,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="0">
+                      <item row="4" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintRadius">
                         <property name="suffix">
                          <string> m</string>
@@ -9931,7 +9943,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
+                      <item row="2" column="1">
                        <widget class="QLabel" name="label_366">
                         <property name="text">
                          <string>Minimum map size.</string>
@@ -9944,7 +9956,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="0">
+                      <item row="3" column="0">
+                       <widget class="QSpinBox" name="spinBox_grid_maxNodes">
+                        <property name="maximum">
+                         <number>9999</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
                        <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minMapSize">
                         <property name="suffix">
                          <string> m</string>
@@ -9966,7 +9985,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="1">
+                      <item row="3" column="1">
                        <widget class="QLabel" name="label_455">
                         <property name="text">
                          <string>Maximum nodes assembled in the map starting from the last node (0=unlimited).</string>
@@ -9979,10 +9998,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="spinBox_grid_maxNodes">
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_454">
+                        <property name="text">
+                         <string>Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_updateError">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.001000000000000</double>
+                        </property>
                         <property name="maximum">
-                         <number>9999</number>
+                         <double>99.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.010000000000000</double>
                         </property>
                        </widget>
                       </item>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.15.4</version>
+  <version>0.16.0</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/Reprocess/main.cpp

@@ -254,14 +254,14 @@ int main(int argc, char * argv[])
 #endif
 						if(updateGridMap || updateOctoMap)
 						{
-							cv::Mat ground, obstacles;
-							stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+							cv::Mat ground, obstacles, empty;
+							stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
 
 							timeUpdateInit = t.ticks();
 
 							if(updateGridMap)
 							{
-								grid.addToCache(id, ground, obstacles);
+								grid.addToCache(id, ground, obstacles, empty);
 								grid.update(stats.poses());
 								timeUpdateGrid = t.ticks() + timeUpdateInit;
 							}
@@ -269,7 +269,7 @@ int main(int argc, char * argv[])
 							if(updateOctoMap)
 							{
 								const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
-								octomap.addToCache(id, ground, obstacles, viewpoint);
+								octomap.addToCache(id, ground, obstacles, empty, viewpoint);
 								octomap.update(stats.poses());
 								timeUpdateOctoMap = t.ticks() + timeUpdateInit;
 							}
@@ -378,6 +378,18 @@ int main(int argc, char * argv[])
 				printf("Saving 3d ground \"%s\"... failed!\n", outputPath.c_str());
 			}
 		}
+		if(grid.getMapEmptyCells()->size())
+		{
+			outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_empty.pcd";
+			if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapEmptyCells()) == 0)
+			{
+				printf("Saving 3d empty cells \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving 3d empty cells \"%s\"... failed!\n", outputPath.c_str());
+			}
+		}
 	}
 #ifdef RTABMAP_OCTOMAP
 	if(assemble2dOctoMap)
@@ -427,8 +439,8 @@ int main(int argc, char * argv[])
 	if(assemble3dOctoMap)
 	{
 		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_occupied.pcd";
-		std::vector<int> obstacles, emptySpace;
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace);
+		std::vector<int> obstacles, emptySpace, ground;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles, &emptySpace, &ground);
 		if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
 		{
 			printf("Saving obstacles cloud \"%s\"... done!\n", outputPath.c_str());
@@ -437,6 +449,18 @@ int main(int argc, char * argv[])
 		{
 			printf("Saving obstacles cloud \"%s\"... failed!\n", outputPath.c_str());
 		}
+		if(ground.size())
+		{
+			outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_ground.pcd";
+			if(pcl::io::savePCDFile(outputPath, *cloud, ground, true) == 0)
+			{
+				printf("Saving empty space cloud \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving empty space cloud \"%s\"... failed!\n", outputPath.c_str());
+			}
+		}
 		if(emptySpace.size())
 		{
 			outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap_empty.pcd";