Improved occupancy grid map construction performance

corelib/include/rtabmap/core/Memory.h

@@ -114,6 +114,7 @@ public:
 	int getMapId(int signatureId) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	Signature getSignatureData(int locationId, bool uncompressedData = false);
+	Signature getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}

corelib/include/rtabmap/core/Rtabmap.h

@@ -88,6 +88,7 @@ public:
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
+	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -431,6 +431,72 @@ pcl::IndicesPtr extractNegativeIndices(
 	return output;
 }
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	if(cloud->size() == 0)
+	{
+		return;
+	}
+	pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+	segmentObstaclesFromGround<PointT>(cloud,
+			groundIndices,
+			obstaclesIndices,
+			cellSize,
+			groundNormalAngle,
+			minClusterSize);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
+		//voxelize to grid cell size
+		groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
+	}
+
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		ground = cv::Mat(groundCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloud->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		obstacles = cv::Mat(obstaclesCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
+		}
+	}
+}
+
 } // util3d
 } // rtabmap
 #endif //UTIL3D_HPP_

corelib/include/rtabmap/core/util3d.h

@@ -346,13 +346,11 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumAngle = 2*M_PI/3,
 		bool gp3NormalConsistency = false);
 
-bool RTABMAP_EXP occupancy2DFromCloud3D(
+void occupancy2DFromLaserScan(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const cv::Mat & scan,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
-		float cellSize = 0.05f,
+		float cellSize);
-		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -360,7 +358,6 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		int fillEmptyRadius = 0,
 		float minMapSize = 0.0f);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
@@ -557,6 +554,15 @@ pcl::IndicesPtr extractNegativeIndices(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices);
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize = 0.05f,
+		float groundNormalAngle = M_PI_4,
+		int minClusterSize = 20);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/Memory.cpp

@@ -2607,6 +2607,56 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
 	return r;
 }
 
+Signature Memory::getSignatureDataConst(int locationId) const
+{
+	UDEBUG("");
+	Signature r;
+	const Signature * s = this->getSignature(locationId);
+	if(s && !s->getImageCompressed().empty())
+	{
+		r = *s;
+	}
+	else if(_dbDriver)
+	{
+		// load from database
+		if(s)
+		{
+			std::list<Signature*> signatures;
+			r = *s;
+			signatures.push_back(&r);
+			_dbDriver->loadNodeData(signatures, true);
+		}
+		else
+		{
+			std::list<int> ids;
+			ids.push_back(locationId);
+			std::list<Signature*> signatures;
+			std::set<int> loadedFromTrash;
+			_dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
+			if(signatures.size())
+			{
+				Signature * sTmp = signatures.front();
+				if(sTmp->getImageCompressed().empty())
+				{
+					_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
+				}
+				r = *sTmp;
+				if(loadedFromTrash.size())
+				{
+					//put it back to trash
+					_dbDriver->asyncSave(sTmp);
+				}
+				else
+				{
+					delete sTmp;
+				}
+			}
+		}
+	}
+
+	return r;
+}
+
 void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
 {
 	if(!_dbDriver)

corelib/src/util3d.cpp

@@ -2006,58 +2006,56 @@ pcl::PolygonMesh::Ptr createMesh(
 	return mesh;
 }
 
-bool occupancy2DFromCloud3D(
+void occupancy2DFromLaserScan(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const cv::Mat & scan,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
-		float cellSize,
+		float cellSize)
-		float groundNormalAngle,
-		int minClusterSize)
 {
-	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	if(scan.empty())
-	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
-
-	//voxelize
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
-
-	pcl::IndicesPtr groundIndices, obstaclesIndices;
-
-	segmentObstaclesFromGround<pcl::PointXYZRGB>(cloud,
-			groundIndices,
-			obstaclesIndices,
-			cellSize,
-			groundNormalAngle,
-			minClusterSize);
-
-	if(groundIndices->size())
 	{
-		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+		return;
-		//project on XY plane
-		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
-		//voxelize to grid cell size
-		groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
 	}
 
-	if(obstaclesIndices->size())
+	std::map<int, Transform> poses;
-	{
+	poses.insert(std::make_pair(1, Transform::getIdentity()));
-		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
-		//voxelize to grid cell size
-		obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
-	}
 
-	ground = cv::Mat();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan);
-	if(groundCloud->size())
+	//obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
+
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
+	scans.insert(std::make_pair(1, obstaclesCloud));
+
+	float xMin, yMin;
+	cv::Mat map8S = create2DMap(poses, scans, cellSize, false, xMin, yMin);
+
+	// find ground cells
+	std::list<int> groundIndices;
+	for(unsigned int i=0; i< map8S.total(); ++i)
 	{
-		ground = cv::Mat(groundCloud->size(), 1, CV_32FC2);
+		if(map8S.data[i] == 0)
-		for(unsigned int i=0;i<groundCloud->size(); ++i)
 		{
-			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
+			groundIndices.push_back(i);
-			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
 		}
 	}
 
+	// Convert to position matrices, get points to each center of the cells
+	ground = cv::Mat();
+	if(groundIndices.size())
+	{
+		ground = cv::Mat(groundIndices.size(), 1, CV_32FC2);
+		int i=0;
+		for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
+		{
+			int x = *iter / map8S.cols;
+			int y = *iter - x*map8S.cols;
+			ground.at<cv::Vec2f>(i)[0] = (float(y)+0.5)*cellSize + xMin;
+			ground.at<cv::Vec2f>(i)[1] = (float(x)+0.5)*cellSize + yMin;
+			++i;
+		}
+	}
+
+	// copy directly obstacles precise positions
 	obstacles = cv::Mat();
 	if(obstaclesCloud->size())
 	{
@@ -2068,27 +2066,6 @@ bool occupancy2DFromCloud3D(
 			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
 		}
 	}
-	/*
-	if(cloud->size())
-	{
-		UWARN("saving cloud");
-		pcl::io::savePCDFile("cloud.pcd", *cloud);
-		pcl::io::savePCDFile("cloudXYZ.pcd", *cloudXYZ);
-	}
-	if(groundCloud->size())
-	{
-		UWARN("saving ground");
-		pcl::io::savePCDFile("ground.pcd", *groundCloud);
-		pcl::io::savePCDFile("ground_indices.pcd", *cloudXYZ, *groundIndices);
-	}
-	if(obstaclesCloud->size())
-	{
-		UWARN("saving obstacles");
-		pcl::io::savePCDFile("obstacles.pcd", *obstaclesCloud);
-		pcl::io::savePCDFile("obstacles_indices.pcd", *cloudXYZ, *obstaclesIndices);
-	}
-	*/
-	return !ground.empty();
 }
 
 /**
@@ -2101,7 +2078,6 @@ bool occupancy2DFromCloud3D(
  * @param cellSize m
  * @param xMin
  * @param yMin
- * @param fillEmptyRadius fill neighbors of empty space if there're no obstacles.
  */
 cv::Mat create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -2109,10 +2085,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		int fillEmptyRadius,
 		float minMapSize)
 {
-	UASSERT(fillEmptyRadius >= 0);
 	UASSERT(minMapSize >= 0.0f);
 	UDEBUG("");
 	UTimer timer;
@@ -2209,7 +2183,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	if(minX != maxX && minY != maxY)
 	{
 		//Get map size
-		float margin = (fillEmptyRadius + 1)*cellSize;
+		float margin = cellSize;
 		xMin = minX-margin;
 		yMin = minY-margin;
 		float xMax = maxX+margin;
@@ -2227,19 +2201,6 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 				{
 					cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
 					map.at<char>(pt.y, pt.x) = 0; // free space
-					if(fillEmptyRadius>0)
-					{
-						for(int j=pt.y-fillEmptyRadius; j<=pt.y+fillEmptyRadius; ++j)
-						{
-							for(int k=pt.x-fillEmptyRadius; k<=pt.x+fillEmptyRadius; ++k)
-							{
-								if(map.at<char>(j, k) == -1)
-								{
-									map.at<char>(j, k) = 0;
-								}
-							}
-						}
-					}
 				}
 			}
 			if(jter!=occupiedLocalMaps.end())
@@ -2252,6 +2213,50 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			}
 			//UDEBUG("empty=%d occupied=%d", empty, occupied);
 		}
+
+		// fill holes and remove empty from obstacle borders
+		cv::Mat updatedMap = map;
+		for(int i=2; i<map.rows-2; ++i)
+		{
+			for(int j=2; j<map.cols-2; ++j)
+			{
+				if(map.at<char>(i, j) == -1 &&
+					map.at<char>(i+1, j) != -1 &&
+					map.at<char>(i-1, j) != -1 &&
+					map.at<char>(i, j+1) != -1 &&
+					map.at<char>(i, j-1) != -1)
+				{
+					updatedMap.at<char>(i, j) = 0;
+				}
+				else if(map.at<char>(i, j) == 100)
+				{
+					// obstacle/empty/unknown -> remove empty
+					// unknown/empty/obstacle -> remove empty
+					if(map.at<char>(i-1, j) == 0 &&
+						map.at<char>(i-2, j) == -1)
+					{
+						updatedMap.at<char>(i-1, j) = -1;
+					}
+					else if(map.at<char>(i+1, j) == 0 &&
+							map.at<char>(i+2, j) == -1)
+					{
+						updatedMap.at<char>(i+1, j) = -1;
+					}
+					if(map.at<char>(i, j-1) == 0 &&
+						map.at<char>(i, j-2) == -1)
+					{
+						updatedMap.at<char>(i, j-1) = -1;
+					}
+					else if(map.at<char>(i, j+1) == 0 &&
+							map.at<char>(i, j+2) == -1)
+					{
+						updatedMap.at<char>(i, j+1) = -1;
+					}
+				}
+
+			}
+		}
+		map = updatedMap;
 	}
 	UDEBUG("timer=%fs", timer.ticks());
 	return map;
@@ -2427,37 +2432,70 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
 	ptB = end;
 
 	float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
+
+	bool swapped = false;
+	if(slope<-1.0f || slope>1.0f)
+	{
+		// swap x and y
+		slope = 1.0f/slope;
+
+		int tmp = ptA.x;
+		ptA.x = ptA.y;
+		ptA.y = tmp;
+
+		tmp = ptB.x;
+		ptB.x = ptB.y;
+		ptB.y = tmp;
+
+		swapped = true;
+	}
+
 	float b = ptA.y - slope*ptA.x;
-
-	//UWARN("start=%d,%d end=%d,%d", ptA.x, ptA.y, ptB.x, ptB.y);
-
-	//ROS_WARN("y = %f*x + %f", slope, b);
 	for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
 	{
-		int lowerbound = float(x)*slope + b;
+		int upperbound = float(x)*slope + b;
-		int upperbound = float(ptA.x<ptB.x?x+1:x-1)*slope + b;
+		int lowerbound = upperbound;
+		if(x != ptA.x)
+		{
+			lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
+		}
 
 		if(lowerbound > upperbound)
 		{
-			int tmp = lowerbound;
+			int tmp = upperbound;
-			lowerbound = upperbound;
+			upperbound = lowerbound;
-			upperbound = tmp;
+			lowerbound = tmp;
 		}
 
-		//ROS_WARN("lowerbound=%f upperbound=%f", lowerbound, upperbound);
+		if(!swapped)
-		UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
+		{
-		UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
+			UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
+			UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
+		}
+		else
+		{
+			UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
+			UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
+		}
 
 		for(int y = lowerbound; y<=(int)upperbound; ++y)
 		{
-			char & v = grid.at<char>(y, x);
+			char * v;
-			if(v == 100 && stopOnObstacle)
+			if(swapped)
+			{
+				v = &grid.at<char>(x, y);
+			}
+			else
+			{
+				v = &grid.at<char>(y, x);
+			}
+			if(*v == 100 && stopOnObstacle)
 			{
 				return;
 			}
 			else
 			{
-				v = 0; // free space
+				*v = 0; // free space
 			}
 		}
 	}

guilib/include/rtabmap/gui/MainWindow.h

@@ -275,7 +275,8 @@ private:
 	std::map<int, int> _currentMapIds;   // <nodeId, mapId>
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > _occupancyLocalMaps; // <ground, obstacles>
+	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
+	std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _lastOdometryProcessed;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -147,9 +147,7 @@ public:
 
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;
-	bool getGridMapFillEmptySpace() const;
 	bool isGridMapFrom3DCloud() const;
-	int getGridMapFillEmptyRadius() const;
 	double getGridMapOpacity() const;
 
 	QString getWorkingDirectory() const;

guilib/src/MainWindow.cpp

@@ -1191,7 +1191,7 @@ void MainWindow::updateMapCloud(
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
 				_ui->actionView_scans->setEnabled(true);
 			}
-			else if(_preferencesDialog->isGridMapFrom3DCloud() && _occupancyLocalMaps.size())
+			else if(_preferencesDialog->isGridMapFrom3DCloud() && _projectionLocalMaps.size())
 			{
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
 			}
@@ -1394,13 +1394,19 @@ void MainWindow::updateMapCloud(
 		cv::Mat map8S;
 		if(_preferencesDialog->isGridMapFrom3DCloud())
 		{
-			int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
+			map8S = util3d::create2DMapFromOccupancyLocalMaps(
-			map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
+					poses,
+					_projectionLocalMaps,
+					resolution,
+					xMin, yMin);
 		}
-		else if(_createdScans.size())
+		else if(_gridLocalMaps.size())
 		{
-			bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
+			map8S = util3d::create2DMapFromOccupancyLocalMaps(
-			map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
+					poses,
+					_gridLocalMaps,
+					resolution,
+					xMin, yMin);
 		}
 		if(!map8S.empty())
 		{
@@ -1506,9 +1512,20 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		float groundNormalMaxAngle = M_PI_4;
 		int minClusterSize = 20;
 		cv::Mat ground, obstacles;
-		if(util3d::occupancy2DFromCloud3D(cloud, ground, obstacles, cellSize, groundNormalMaxAngle, minClusterSize))
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = cloud;
+		if(voxelizedCloud->size())
 		{
-			_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+			voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
+		}
+		util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
+				voxelizedCloud,
+				ground, obstacles,
+				cellSize,
+				groundNormalMaxAngle,
+				minClusterSize);
+		if(!ground.empty() || !obstacles.empty())
+		{
+			_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 		}
 		UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
 	}
@@ -1607,6 +1624,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		else
 		{
 			_createdScans.insert(std::make_pair(nodeId, cloud));
+
+			cv::Mat ground, obstacles;
+			util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
+			_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
 		}
 		_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 		_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
@@ -3504,7 +3525,8 @@ void MainWindow::clearTheCache()
 	_cachedSignatures.clear();
 	_createdClouds.clear();
 	_createdScans.clear();
-	_occupancyLocalMaps.clear();
+	_gridLocalMaps.clear();
+	_projectionLocalMaps.clear();
 	_ui->widget_cloudViewer->removeAllClouds();
 	_ui->widget_cloudViewer->removeAllGraphs();
 	_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
@@ -3724,7 +3746,6 @@ void MainWindow::setAspectRatio1080p()
 void MainWindow::exportGridMap()
 {
 	double gridCellSize = 0.05;
-	bool gridUnknownSpaceFilled = true;
 	bool ok;
 	gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
 	if(!ok)
@@ -3732,18 +3753,6 @@ void MainWindow::exportGridMap()
 		return;
 	}
 
-	QMessageBox::StandardButton b = QMessageBox::question(this,
-			tr("Fill empty space?"),
-			tr("Do you want to fill empty space?"),
-			QMessageBox::No | QMessageBox::Yes,
-			QMessageBox::Yes);
-
-	if(b != QMessageBox::Yes && b != QMessageBox::No)
-	{
-		return;
-	}
-	gridUnknownSpaceFilled = b == QMessageBox::Yes;
-
 	std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
 
 	// create the map
@@ -3751,11 +3760,19 @@ void MainWindow::exportGridMap()
 	cv::Mat pixels;
 	if(_preferencesDialog->isGridMapFrom3DCloud())
 	{
-		pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, gridCellSize, xMin, yMin, gridUnknownSpaceFilled?1:0);
+		pixels = util3d::create2DMapFromOccupancyLocalMaps(
+				poses,
+				_projectionLocalMaps,
+				gridCellSize,
+				xMin, yMin);
 	}
 	else
 	{
-		pixels = util3d::create2DMap(poses, _createdScans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
+		pixels = util3d::create2DMapFromOccupancyLocalMaps(
+				poses,
+				_gridLocalMaps,
+				gridCellSize,
+				xMin, yMin);
 	}
 
 	if(!pixels.empty())

guilib/src/PreferencesDialog.cpp

@@ -224,9 +224,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_map_fillEmptySpace, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->spinbox_map_fillEmptyRadius, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	//Logging panel
@@ -854,9 +852,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
-		_ui->checkBox_map_fillEmptySpace->setChecked(true);
 		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->spinbox_map_fillEmptyRadius->setValue(1);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1094,9 +1090,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 
 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
-	_ui->checkBox_map_fillEmptySpace->setChecked(settings.value("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked()).toBool());
 	_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
-	_ui->spinbox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value()).toInt());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
 
 	settings.endGroup(); // General
@@ -1343,9 +1337,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
 
 	settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
-	settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
 	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
-	settings.setValue("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value());
 	settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
 	settings.endGroup(); // General
 
@@ -3000,18 +2992,10 @@ double PreferencesDialog::getGridMapResolution() const
 {
 	return _ui->doubleSpinBox_map_resolution->value();
 }
-bool PreferencesDialog::getGridMapFillEmptySpace() const
-{
-	return _ui->checkBox_map_fillEmptySpace->isChecked();
-}
 bool PreferencesDialog::isGridMapFrom3DCloud() const
 {
 	return _ui->checkBox_map_occupancyFrom3DCloud->isChecked();
 }
-int PreferencesDialog::getGridMapFillEmptyRadius() const
-{
-	return _ui->spinbox_map_fillEmptyRadius->value();
-}
 double PreferencesDialog::getGridMapOpacity() const
 {
 	return _ui->doubleSpinBox_map_opacity->value();

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>744</width>
-            <height>1074</height>
+            <height>1056</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -421,6 +421,23 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        </item>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
+                         <item row="0" column="0">
+                          <widget class="QCheckBox" name="checkBox_map_shown">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_map_shown">
+                           <property name="text">
+                            <string>Show in 3D map view.</string>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="1" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_map_resolution">
                            <property name="suffix">
@@ -444,23 +461,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
-                          <widget class="QLabel" name="label_164">
-                           <property name="text">
-                            <string>Fill empty space.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="0">
-                          <widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
-                           <property name="text">
-                            <string/>
-                           </property>
-                           <property name="checked">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="2" column="1">
                           <widget class="QLabel" name="label_170">
                            <property name="text">
@@ -468,24 +468,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="0" column="0">
+                         <item row="3" column="0">
-                          <widget class="QCheckBox" name="checkBox_map_shown">
-                           <property name="text">
-                            <string/>
-                           </property>
-                           <property name="checked">
-                            <bool>false</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="1">
-                          <widget class="QLabel" name="label_map_shown">
-                           <property name="text">
-                            <string>Show in 3D map view.</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="4" column="0">
                           <widget class="QCheckBox" name="checkBox_map_occupancyFrom3DCloud">
                            <property name="text">
                             <string/>
@@ -511,7 +494,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="4" column="1">
+                         <item row="3" column="1">
                           <widget class="QLabel" name="label_210">
                            <property name="text">
                             <string>Occupancy from 3D cloud projection on the ground. Laser scans are ignored when activated.</string>
@@ -521,32 +504,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="1">
-                          <widget class="QLabel" name="label_211">
-                           <property name="text">
-                            <string>Fill empty radius. Used when occupancy from 3D projection is activated.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="5" column="0">
-                          <widget class="QSpinBox" name="spinbox_map_fillEmptyRadius">
-                           <property name="suffix">
-                            <string> cells</string>
-                           </property>
-                           <property name="minimum">
-                            <number>0</number>
-                           </property>
-                           <property name="maximum">
-                            <number>10</number>
-                           </property>
-                           <property name="value">
-                            <number>1</number>
-                           </property>
-                          </widget>
-                         </item>
                         </layout>
                        </item>
                       </layout>