Added util3d::occupancy2DFromCloud3D() and util3d::create2DMapFromOccupancyLocalMaps() methods to create 2D occupancy maps from 3D clouds

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1899 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Memory.h

@@ -203,7 +203,8 @@ private:
 	void copyData(const Signature * from, Signature * to);
 	Signature * createSignature(
 			const SensorData & data,
-			bool keepRawData=false);
+			bool keepRawData=false,
+			Statistics * stats = 0);
 
 	//keypoint stuff
 	void disableWordsRef(int signatureId);

corelib/include/rtabmap/core/Statistics.h

@@ -99,6 +99,15 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Pre_update, ms);
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);
 	RTABMAP_STATS(TimingMem, Rehearsal, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
+	RTABMAP_STATS(TimingMem, Stereo_subpixel, ms);
+	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_filtering, ms);
+	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
+	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
+	RTABMAP_STATS(TimingMem, Add_new_words, ms);
+	RTABMAP_STATS(TimingMem, Compressing_data, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Response_threshold,);

corelib/include/rtabmap/core/util3d.h

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/PolygonMesh.h>
+#include <pcl/pcl_base.h>
 
 namespace rtabmap
 {
@@ -509,6 +510,22 @@ std::multimap<int, int> RTABMAP_EXP radiusPosesClustering(
 		float radius,
 		float angle);
 
+bool RTABMAP_EXP occupancy2DFromCloud3D(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize = 0.05f,
+		float groundNormalAngle = M_PI_4,
+		int minClusterSize = 20);
+
+cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
+		const std::map<int, Transform> & poses,
+		const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
+		float cellSize,
+		float & xMin,
+		float & yMin,
+		int fillEmptyRadius = 0);
+
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
 		float cellSize,
@@ -523,6 +540,128 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
+void RTABMAP_EXP projectCloudOnXYPlane(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+
+/**
+ * For convenience.
+ */
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * @brief Wrapper of the pcl::RadiusOutlierRemoval class.
+ *
+ * Points in the cloud which have less than a minimum of neighbors in the
+ * specified radius are filtered.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @param minNeighborsInRadius the minimum of neighbors to keep the point.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * For convenience.
+ */
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * @brief Given a normal and a maximum angle error, keep all points of the cloud
+ * respecting this normal.
+ *
+ * The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then
+ * for each normal, the corresponding point is filtered if the
+ * angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum
+ * angle specified by the user.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param angleMax the maximum angle.
+ * @param normal the normal to which each point's normal is compared.
+ * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
+ * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
+ * @return the indices of the points which respect the normal constraint.
+ */
+pcl::IndicesPtr RTABMAP_EXP normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * For convenience.
+ */
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+/**
+ * @brief Wrapper of the pcl::EuclideanClusterExtraction class.
+ *
+ * Extract all clusters from a point cloud given a maximum cluster distance tolerance.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
+ * @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
+ * @return the indices of each cluster found.
+ */
+std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+/**
+ * @brief Concatenate a vector of indices to a single vector.
+ *
+ * @param indices the vector of indices to concatenate.
+ * @note This methods doesn't check if indices exist in the two set and doesn't
+ * sort the output indices. If we are not sure if the the
+ * two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices().
+ * @return the indices concatenated.
+ */
+pcl::IndicesPtr RTABMAP_EXP concatenate(
+		const std::vector<pcl::IndicesPtr> & indices);
+
+/**
+ * @brief Concatenate two vector of indices to a single vector.
+ *
+ * @param indicesA the first vector of indices to concatenate.
+ * @param indicesB the second vector of indices to concatenate.
+ * @note This methods doesn't check if indices exist in the two set and doesn't
+ * sort the output indices. If we are not sure if the the
+ * two set of indices set are disjoint and/or you need sorted indices, the use of mergeIndices().
+ * @return the indices concatenated.
+ */
+pcl::IndicesPtr RTABMAP_EXP concatenate(
+		const pcl::IndicesPtr & indicesA,
+		const pcl::IndicesPtr & indicesB);
+
+pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/src/Memory.cpp

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Statistics.h"
 
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/common.h>
 
 namespace rtabmap {
 
@@ -492,7 +493,7 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	//============================================================
 	// Create a signature with the image received.
 	//============================================================
-	Signature * signature = this->createSignature(data, this->isRawDataKept());
+	Signature * signature = this->createSignature(data, this->isRawDataKept(), stats);
 	if (signature == 0)
 	{
 		UERROR("Failed to create a signature...");
@@ -3055,7 +3056,7 @@ private:
 	VWDictionary * _vwp;
 };
 
-Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
+Signature * Memory::createSignature(const SensorData & data, bool keepRawData, Statistics * stats)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
 	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
@@ -3067,6 +3068,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 
 	UTimer timer;
 	timer.start();
+	float t;
 	std::vector<cv::KeyPoint> keypoints;
 	cv::Mat descriptors;
 	int id = data.id();
@@ -3135,7 +3137,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 				//stereo
 				cv::Mat disparity;
 				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
-				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 				std::vector<cv::Point2f> leftCorners;
 				cv::KeyPoint::convert(keypoints, leftCorners);
@@ -3145,7 +3149,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 							cv::Size( _stereoSubPixWinSize, _stereoSubPixWinSize ),
 							cv::Size( -1, -1 ),
 							cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _stereoSubPixIterations, _stereoSubPixEps ) );
-					UDEBUG("time subpix left kpts=%fs", timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
+					UDEBUG("time subpix left kpts=%fs", t);
 				}
 
 				//generate a disparity map
@@ -3157,68 +3163,90 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 						_stereoFlowMaxLevel,
 						_stereoFlowIterations,
 						_stereoFlowEpsilon);
-				UDEBUG("generate disparity = %fs", timer.ticks());
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
+				UDEBUG("generate disparity = %fs", t);
 
 				if(_wordsMaxDepth > 0.0f)
 				{
 					// disparity = baseline * fx / depth;
 					float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
 					Feature2D::filterKeypointsByDisparity(keypoints, disparity, minDisparity);
-					UDEBUG("time filter keypoints by disparity (%d) = %fs", (int)keypoints.size(), timer.ticks());
+					UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
 				}
 
 				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
 				{
 					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-					UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+					UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
 				}
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+				UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
 
 				if(keypoints.size())
 				{
 					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
 					keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
-					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
 				}
 			}
 			else if(!data.depth().empty())
 			{
 				//depth
 				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
-				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 				if(_wordsMaxDepth > 0.0f)
 				{
 					Feature2D::filterKeypointsByDepth(keypoints, data.depth(), _wordsMaxDepth);
-					UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
+					UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
 				}
 
 				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
 				{
 					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-					UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+					UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
 				}
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+				UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
 
 				if(keypoints.size())
 				{
 					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
 					keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
-					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
 				}
 			}
 			else
 			{
 				//RGB only
 				keypoints = _feature2D->generateKeypoints(imageMono, _wordsPerImageTarget, roi);
-				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				t = timer.ticks();
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
 
 				if(keypoints.size())
 				{
 					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 				}
 			}
 
@@ -3263,7 +3291,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 					_stereoFlowMaxLevel,
 					_stereoFlowIterations,
 					_stereoFlowEpsilon);
-			UDEBUG("generate disparity = %fs", timer.ticks());
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
+			UDEBUG("generate disparity = %fs", t);
 
 			if(_wordsMaxDepth)
 			{
@@ -3275,11 +3305,16 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
 			{
 				Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-				UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+				UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
 			}
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+			UDEBUG("time keypoints filtering=%fs", t);
 
 			keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
 		}
 		else if(!data.depth().empty())
 		{
@@ -3287,22 +3322,30 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			if(_wordsMaxDepth)
 			{
 				Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMaxDepth);
-				UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
 			}
 
 			if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
 			{
 				Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-				UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+				UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
 			}
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+			UDEBUG("time keypoints filtering=%fs", t);
 
 			keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
 		}
 		else
 		{
 			// RGB only
 			Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+			t = timer.ticks();
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
+			UDEBUG("time keypoints filtering=%fs", t);
 		}
 	}
 
@@ -3314,17 +3357,21 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 	std::list<int> wordIds;
 	if(descriptors.rows)
 	{
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
 		if(_parallelized)
 		{
-			UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
+			UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
 		}
 		else
 		{
-			UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, timer.ticks());
+			UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
 		}
 
 		wordIds = _vwd->addNewWords(descriptors, id);
-		UDEBUG("time addNewWords %fs", timer.ticks());
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
+		UDEBUG("time addNewWords %fs", t);
 	}
 	else if(id>0)
 	{
@@ -3402,8 +3449,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 	}
 
 
-
-	UDEBUG("time new signature (id=%d) %fs", id, timer.ticks());
+	t = timer.ticks();
+	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
+	UDEBUG("time compressing data (id=%d) %fs", id, t);
 	if(words.size())
 	{
 		s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())

corelib/src/util3d.cpp

@@ -41,6 +41,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>
 #include <pcl/surface/mls.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/filters/crop_box.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/filters/extract_indices.h>
 
 #include <opencv2/nonfree/features2d.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
@@ -2477,6 +2482,299 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 	return clusters;
 }
 
+bool occupancy2DFromCloud3D(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	//voxelize
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize(cloud, cellSize);
+
+	//convert to XYZ
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::copyPointCloud(*voxelizedCloud, *cloudXYZ);
+
+	pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+	// Find the ground
+	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering(
+			cloudXYZ,
+			groundNormalAngle,
+			Eigen::Vector4f(0,0,1,0),
+			cellSize*2.0f,
+			Eigen::Vector4f(0,0,100,0));
+
+	int biggestFlatSurfaceIndex;
+	std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters(
+			cloudXYZ,
+			flatSurfaces,
+			cellSize*2.0f,
+			minClusterSize,
+			std::numeric_limits<int>::max(),
+			&biggestFlatSurfaceIndex);
+
+
+	// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
+	groundIndices = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+	Eigen::Vector4f min,max;
+	pcl::getMinMax3D(*cloudXYZ, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+	for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
+	{
+		if((int)i!=biggestFlatSurfaceIndex)
+		{
+			Eigen::Vector4f centroid;
+			pcl::compute3DCentroid(*cloudXYZ, *clusteredFlatSurfaces.at(i), centroid);
+			if(centroid[2] >= min[2] && centroid[2] <= max[2])
+			{
+				groundIndices = util3d::concatenate(groundIndices, clusteredFlatSurfaces.at(i));
+			}
+		}
+	}
+
+	pcl::copyPointCloud(*cloudXYZ, *groundIndices, *groundCloud);
+
+	if(groundIndices->size() != cloudXYZ->size())
+	{
+		// Remove ground
+		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloudXYZ, groundIndices);
+
+		//Cluster remaining stuff (obstacles)
+		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+				cloudXYZ,
+				otherStuffIndices,
+				cellSize*2.0f,
+				minClusterSize);
+
+		// merge indices
+		obstaclesIndices = util3d::concatenate(clusteredObstaclesSurfaces);
+		if(obstaclesIndices->size())
+		{
+			pcl::copyPointCloud(*cloudXYZ, *obstaclesIndices, *obstaclesCloud);
+		}
+	}
+
+	//project on XY plane
+	util3d::projectCloudOnXYPlane(groundCloud);
+	util3d::projectCloudOnXYPlane(obstaclesCloud);
+
+	//voxelize to grid cell size
+	groundCloud = util3d::voxelize(groundCloud, cellSize);
+	obstaclesCloud = util3d::voxelize(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;
+		}
+	}
+	/*
+	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();
+}
+
+/**
+ * Create 2d Occupancy grid (CV_8S) from 2d occupancy
+ * -1 = unknown
+ * 0 = empty space
+ * 100 = obstacle
+ * @param poses
+ * @param occupancy <empty, occupied>
+ * @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,
+		const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
+		float cellSize,
+		float & xMin,
+		float & yMin,
+		int fillEmptyRadius)
+{
+	UASSERT(fillEmptyRadius >= 0);
+	UDEBUG("");
+	UTimer timer;
+
+	std::map<int, cv::Mat> emptyLocalMaps;
+	std::map<int, cv::Mat> occupiedLocalMaps;
+
+	float minX=0.0f, minY=0.0f, maxX=0.0f, maxY=0.0f;
+	bool undefinedSize = true;
+	float x,y,z,toll,pitch,yaw,cosT,sinT;
+	cv::Mat affineTransform(2,3,CV_32FC1);
+	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(uContains(occupancy, iter->first))
+		{
+			const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
+
+			iter->second.getTranslationAndEulerAngles(x,y,z,toll,pitch,yaw);
+			cosT = cos(yaw);
+			sinT = sin(yaw);
+			affineTransform.at<float>(0,0) = cosT;
+			affineTransform.at<float>(0,1) = -sinT;
+			affineTransform.at<float>(1,0) = sinT;
+			affineTransform.at<float>(1,1) = cosT;
+			affineTransform.at<float>(0,2) = x;
+			affineTransform.at<float>(1,2) = y;
+
+			if(undefinedSize)
+			{
+				minX = maxX = x;
+				minY = maxY = y;
+				undefinedSize = false;
+			}
+			else
+			{
+				if(minX > x)
+					minX = x;
+				else if(maxX < x)
+					maxX = x;
+
+				if(minY > y)
+					minY = y;
+				else if(maxY < y)
+					maxY = y;
+			}
+
+			//ground
+			if(pair.first.rows)
+			{
+				UASSERT(pair.first.type() == CV_32FC2);
+				cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
+				cv::transform(pair.first, ground, affineTransform);
+				for(int i=0; i<ground.rows; ++i)
+				{
+					if(minX > ground.at<float>(i,0))
+						minX = ground.at<float>(i,0);
+					else if(maxX < ground.at<float>(i,0))
+						maxX = ground.at<float>(i,0);
+
+					if(minY > ground.at<float>(i,1))
+						minY = ground.at<float>(i,1);
+					else if(maxY < ground.at<float>(i,1))
+						maxY = ground.at<float>(i,1);
+				}
+				emptyLocalMaps.insert(std::make_pair(iter->first, ground));
+			}
+
+			//obstacles
+			if(pair.second.rows)
+			{
+				UASSERT(pair.second.type() == CV_32FC2);
+				cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
+				cv::transform(pair.second, obstacles, affineTransform);
+				for(int i=0; i<obstacles.rows; ++i)
+				{
+					if(minX > obstacles.at<float>(i,0))
+						minX = obstacles.at<float>(i,0);
+					else if(maxX < obstacles.at<float>(i,0))
+						maxX = obstacles.at<float>(i,0);
+
+					if(minY > obstacles.at<float>(i,1))
+						minY = obstacles.at<float>(i,1);
+					else if(maxY < obstacles.at<float>(i,1))
+						maxY = obstacles.at<float>(i,1);
+				}
+				occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
+			}
+		}
+	}
+	UDEBUG("timer=%fs", timer.ticks());
+
+	cv::Mat map;
+	if(minX != maxX && minY != maxY)
+	{
+		//Get map size
+		float margin = fillEmptyRadius + 1;
+		xMin = minX-margin;
+		yMin = minY-margin;
+		float xMax = maxX+margin;
+		float yMax = maxY+margin;
+		UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
+
+		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
+		for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
+		{
+			std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
+			std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
+			if(iter!=emptyLocalMaps.end())
+			{
+				for(int i=0; i<iter->second.rows; ++i)
+				{
+					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())
+			{
+				for(int i=0; i<jter->second.rows; ++i)
+				{
+					cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
+					map.at<char>(pt.y, pt.x) = 100; // obstacles
+				}
+			}
+			//UDEBUG("empty=%d occupied=%d", empty, occupied);
+		}
+	}
+	UDEBUG("timer=%fs", timer.ticks());
+	return map;
+}
+
 /**
  * Create 2d Occupancy grid (CV_8S)
  * -1 = unknown
@@ -2496,6 +2794,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		float & xMin,
 		float & yMin)
 {
+	UDEBUG("");
 	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
 
 	pcl::PointCloud<pcl::PointXYZ> minMax;
@@ -2540,12 +2839,15 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 				map.at<char>(end.y, end.x) = 100; // obstacle
 				rayTrace(start, end, map, true); // trace free space
 
-				float dx = iter->second->points[i].x - pose.x();
-				float dy = iter->second->points[i].y - pose.y();
-				float l = dx*dx + dy*dy;
-				if(l > maxSquaredLength[j])
+				if(unknownSpaceFilled)
 				{
-					maxSquaredLength[j] = l;
+					float dx = iter->second->points[i].x - pose.x();
+					float dy = iter->second->points[i].y - pose.y();
+					float l = dx*dx + dy*dy;
+					if(l > maxSquaredLength[j])
+					{
+						maxSquaredLength[j] = l;
+					}
 				}
 			}
 			++j;
@@ -2706,6 +3008,240 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	return map8U;
 }
 
+void projectCloudOnXYPlane(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
+{
+	for(unsigned int i=0; i<cloud->size(); ++i)
+	{
+		cloud->at(i).z = 0;
+	}
+}
+
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
+}
+
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		tree->setInputCloud(cloud, indices);
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			if(k > minNeighborsInRadius)
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		tree->setInputCloud(cloud);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			if(k > minNeighborsInRadius)
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
+pcl::IndicesPtr normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+}
+
+pcl::IndicesPtr normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint)
+{
+	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
+	ne.setInputCloud (cloud);
+	if(indices->size())
+	{
+		ne.setIndices(indices);
+	}
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
+	if(indices->size())
+	{
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud(cloud);
+	}
+	ne.setSearchMethod (tree);
+
+	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
+
+	ne.setRadiusSearch (radiusSearch);
+	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
+	{
+		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
+	}
+
+	ne.compute (*cloud_normals);
+
+	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
+	int oi = 0; // output iterator
+	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
+	for(unsigned int i=0; i<cloud_normals->size(); ++i)
+	{
+		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
+		float angle = pcl::getAngle3D(normal, v);
+		if(angle < angleMax)
+		{
+			output->at(oi++) = indices->size()!=0?indices->at(i):i;
+		}
+	}
+	output->resize(oi);
+
+	return output;
+}
+
+std::vector<pcl::IndicesPtr> extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize,
+		int * biggestClusterIndex)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
+}
+
+std::vector<pcl::IndicesPtr> extractClusters(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize,
+		int * biggestClusterIndex)
+{
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
+	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+	ec.setClusterTolerance (clusterTolerance);
+	ec.setMinClusterSize (minClusterSize);
+	ec.setMaxClusterSize (maxClusterSize);
+	ec.setInputCloud (cloud);
+
+	if(indices->size())
+	{
+		ec.setIndices(indices);
+		kdTree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		kdTree->setInputCloud(cloud);
+	}
+	ec.setSearchMethod (kdTree);
+
+	std::vector<pcl::PointIndices> cluster_indices;
+	ec.extract (cluster_indices);
+
+	int maxIndex=-1;
+	unsigned int maxSize = 0;
+	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
+	for(unsigned int i=0; i<cluster_indices.size(); ++i)
+	{
+		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
+
+		if(maxSize < cluster_indices[i].indices.size())
+		{
+			maxSize = cluster_indices[i].indices.size();
+			maxIndex = i;
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = maxIndex;
+	}
+
+	return output;
+}
+
+pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
+{
+	//compute total size
+	unsigned int totalSize = 0;
+	for(unsigned int i=0; i<indices.size(); ++i)
+	{
+		totalSize += indices[i]->size();
+	}
+	pcl::IndicesPtr ind(new std::vector<int>(totalSize));
+	unsigned int io = 0;
+	for(unsigned int i=0; i<indices.size(); ++i)
+	{
+		for(unsigned int j=0; j<indices[i]->size(); ++j)
+		{
+			ind->at(io++) = indices[i]->at(j);
+		}
+	}
+	return ind;
+}
+
+pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
+{
+	pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
+	ind->resize(ind->size()+indicesB->size());
+	unsigned int oi = indicesA->size();
+	for(unsigned int i=0; i<indicesB->size(); ++i)
+	{
+		ind->at(oi++) = indicesB->at(i);
+	}
+	return ind;
+}
+
+pcl::IndicesPtr extractNegativeIndices(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices)
+{
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::ExtractIndices<pcl::PointXYZ> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(true);
+	extract.filter(*output);
+	return output;
+}
+
 }
 
 }

guilib/include/rtabmap/gui/MainWindow.h

@@ -258,8 +258,9 @@ private:
 	QMap<int, int> _mapIds;
 	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
-	QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
+	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>
 	Transform _odometryCorrection;
 	Transform _lastOdomPose;
 	bool _lastOdometryProcessed;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -145,6 +145,8 @@ 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

@@ -267,7 +267,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
 	connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
 	connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
-	connect(_ui->actionExport_2D_scans_ply_bmp, SIGNAL(triggered()), this, SLOT(exportScans()));
+	connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans()));
 	connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
 	connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans()));
 	connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
@@ -277,7 +277,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 
 	_ui->actionPause->setShortcut(Qt::Key_Space);
 	_ui->actionSave_point_cloud->setEnabled(false);
-	_ui->actionExport_2D_scans_ply_bmp->setEnabled(false);
+	_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
 	_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
 	_ui->actionView_scans->setEnabled(false);
 	_ui->actionView_high_res_point_cloud->setEnabled(false);
@@ -1087,10 +1087,14 @@ void MainWindow::updateMapCloud(
 
 			if(_depths2DMap.size())
 			{
-				_ui->actionExport_2D_scans_ply_bmp->setEnabled(true);
+				_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
 				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
 				_ui->actionView_scans->setEnabled(true);
 			}
+			else if(_preferencesDialog->isGridMapFrom3DCloud() && _occupancyLocalMaps.size())
+			{
+				_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
+			}
 		}
 	}
 
@@ -1219,12 +1223,21 @@ void MainWindow::updateMapCloud(
 		_ui->graphicsView_graphView->updateGraph(poses, constraints);
 	}
 	cv::Mat map8U;
-	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && _depths2DMap.size())
+	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();
-		bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
-		cv::Mat map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
+		cv::Mat map8S;
+		if(_preferencesDialog->isGridMapFrom3DCloud())
+		{
+			int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
+			map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
+		}
+		else
+		{
+			bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
+			map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
+		}
 		if(!map8S.empty())
 		{
 			//convert to gray scaled map
@@ -1303,6 +1316,18 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 			_preferencesDialog->getCloudDecimation(0),
 			_preferencesDialog->getCloudMaxDepth(0));
 
+	if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
+	{
+		float cellSize = _preferencesDialog->getGridMapResolution();
+		float groundNormalMaxAngle = M_PI_4;
+		int minClusterSize = 20;
+		cv::Mat ground, obstacles;
+		if(util3d::occupancy2DFromCloud3D(cloud, ground, obstacles, cellSize, groundNormalMaxAngle, minClusterSize))
+		{
+			_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+		}
+	}
+
 	if(_preferencesDialog->isCloudMeshing())
 	{
 		pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
@@ -1330,7 +1355,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 			}
 			else
 			{
-				_createdClouds.insert(nodeId, tmp);
+				_createdClouds.insert(std::make_pair(nodeId, tmp));
 			}
 		}
 	}
@@ -1355,7 +1380,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 		}
 		else
 		{
-			_createdClouds.insert(nodeId, cloud);
+			_createdClouds.insert(std::make_pair(nodeId, cloud));
 		}
 	}
 
@@ -2765,6 +2790,7 @@ void MainWindow::clearTheCache()
 	_localTransformsMap.clear();
 	_createdClouds.clear();
 	_createdScans.clear();
+	_occupancyLocalMaps.clear();
 	_ui->widget_cloudViewer->removeAllClouds();
 	_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
 	_ui->widget_cloudViewer->clearTrajectory();
@@ -2773,7 +2799,7 @@ void MainWindow::clearTheCache()
 	_lastOdomPose.setNull();
 	//disable save cloud action
 	_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
-	_ui->actionExport_2D_scans_ply_bmp->setEnabled(false);
+	_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
 	_ui->actionSave_point_cloud->setEnabled(false);
 	_ui->actionView_scans->setEnabled(false);
 	_ui->actionView_high_res_point_cloud->setEnabled(false);
@@ -2992,22 +3018,19 @@ void MainWindow::exportGridMap()
 	}
 	gridUnknownSpaceFilled = b == QMessageBox::Yes;
 
-	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans;
-	std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
-	std::map<int, Transform> poses;
-	for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
-	{
-		if(_depths2DMap.contains(iter->first))
-		{
-			cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(iter->first));
-			scans.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d)));
-			poses.insert(*iter);
-		}
-	}
+	std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
 
 	// create the map
 	float xMin=0.0f, yMin=0.0f;
-	cv::Mat pixels = util3d::create2DMap(poses, scans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
+	cv::Mat pixels;
+	if(_preferencesDialog->isGridMapFrom3DCloud())
+	{
+		pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, gridCellSize, xMin, yMin, gridUnknownSpaceFilled?1:0);
+	}
+	else
+	{
+		pixels = util3d::create2DMap(poses, _createdScans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
+	}
 
 	if(!pixels.empty())
 	{
@@ -3849,9 +3872,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 							regenerateDecimation,
 							regenerateMaxDepth);
 				}
-				else if(_createdClouds.contains(iter->first))
+				else if(uContains(_createdClouds, iter->first))
 				{
-					cloud = util3d::transformPointCloud(_createdClouds.value(iter->first), iter->second);
+					cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
 				}
 
 				if(cloud->size())
@@ -3932,9 +3955,9 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 							regenerateDecimation,
 							regenerateMaxDepth);
 				}
-				else if(_createdClouds.contains(iter->first))
+				else if(uContains(_createdClouds, iter->first))
 				{
-					cloud = _createdClouds.value(iter->first);
+					cloud = _createdClouds.at(iter->first);
 				}
 
 				if(cloud->size())

guilib/src/PreferencesDialog.cpp

@@ -818,6 +818,8 @@ 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->checkBox_map_fillEmptyRadius->setValue(0);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1053,6 +1055,8 @@ 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->checkBox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value()).toInt());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
 
 	settings.endGroup(); // General
@@ -1293,6 +1297,8 @@ 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->checkBox_map_fillEmptyRadius->value());
 	settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
 	settings.endGroup(); // General
 
@@ -2653,6 +2659,14 @@ 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->checkBox_map_fillEmptyRadius->value();
+}
 double PreferencesDialog::getGridMapOpacity() const
 {
 	return _ui->doubleSpinBox_map_opacity->value();

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>21</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -35,7 +35,7 @@
      <string>File</string>
     </property>
     <addaction name="actionSave_point_cloud"/>
-    <addaction name="actionExport_2D_scans_ply_bmp"/>
+    <addaction name="actionExport_2D_scans_ply_pcd"/>
     <addaction name="actionExport_2D_Grid_map_bmp_png"/>
     <addaction name="separator"/>
     <addaction name="actionExit"/>
@@ -1053,7 +1053,7 @@
     <string>Export 2D grid map (*.png *.bmp)...</string>
    </property>
   </action>
-  <action name="actionExport_2D_scans_ply_bmp">
+  <action name="actionExport_2D_scans_ply_pcd">
    <property name="text">
     <string>Export 2D scans (*.ply *.pcd)...</string>
    </property>

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>744</width>
-            <height>900</height>
+            <height>1047</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -391,7 +391,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="0" column="1">
                           <widget class="QLabel" name="label_37">
                            <property name="text">
-                            <string>Radius</string>
+                            <string>Radius.</string>
                            </property>
                           </widget>
                          </item>
@@ -414,7 +414,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_48">
                            <property name="text">
-                            <string>Angle</string>
+                            <string>Angle.</string>
                            </property>
                           </widget>
                          </item>
@@ -438,7 +438,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <item>
                         <widget class="QLabel" name="label_167">
                          <property name="text">
-                          <string>When laser scans are used, a 2D grid map can be created. When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated.</string>
+                          <string>When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated using the laser scans.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -466,18 +466,18 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="1" column="1">
                           <widget class="QLabel" name="label_159">
                            <property name="text">
-                            <string>Resolution (cell size)</string>
+                            <string>Resolution (cell size).</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
+                         <item row="3" column="1">
                           <widget class="QLabel" name="label_164">
                            <property name="text">
-                            <string>Fill empty space</string>
+                            <string>Fill empty space.</string>
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
                            <property name="text">
                             <string/>
@@ -487,26 +487,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
-                           <property name="suffix">
-                            <string/>
-                           </property>
-                           <property name="minimum">
-                            <double>0.750000000000000</double>
-                           </property>
-                           <property name="maximum">
-                            <double>1.000000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.750000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="3" column="1">
+                         <item row="2" column="1">
                           <widget class="QLabel" name="label_170">
                            <property name="text">
-                            <string>Opacity</string>
+                            <string>Opacity.</string>
                            </property>
                           </widget>
                          </item>
@@ -523,7 +507,69 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          <item row="0" column="1">
                           <widget class="QLabel" name="label_map_shown">
                            <property name="text">
-                            <string>Show in 3D map view</string>
+                            <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/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
+                           <property name="suffix">
+                            <string/>
+                           </property>
+                           <property name="minimum">
+                            <double>0.750000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.750000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" 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>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </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="checkBox_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>0</number>
                            </property>
                           </widget>
                          </item>