OccupancyGrid: added Grid/Footprint*** parameters, templated segmentCloud() function, added Grid/ClusterRadius parameter. Parameters: fixed getDefaultParameters(group) function to correctly compare groups.

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
+#include <pcl/point_cloud.h>
+#include <pcl/pcl_base.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Signature.h>
 
@@ -42,7 +44,22 @@ public:
 	void parseParameters(const ParametersMap & parameters);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
-	void createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const;
+
+	template<typename PointT>
+	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
+			const typename pcl::PointCloud<PointT>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			const Transform & pose,
+			const cv::Point3f & viewPoint,
+			pcl::IndicesPtr & groundIndices,        // output cloud indices
+			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
+			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
+
+	void createLocalMap(
+			const Signature & node,
+			cv::Mat & ground,
+			cv::Mat & obstacles,
+			cv::Point3f & viewPoint) const;
 
 	void clear();
 	void addToCache(
@@ -63,6 +80,9 @@ private:
 	float cloudMaxDepth_;
 	float cloudMinDepth_;
 	std::vector<float> roiRatios_;
+	float footprintLength_;
+	float footprintWidth_;
+	float footprintHeight_;
 	int scanDecimation_;
 	float cellSize_;
 	bool occupancyFromCloud_;
@@ -70,6 +90,7 @@ private:
 	float maxObstacleHeight_;
 	int normalKSearch_;
 	float maxGroundAngle_;
+	float clusterRadius_;
 	int minClusterSize_;
 	bool flatObstaclesDetected_;
 	float minGroundHeight_;
@@ -93,4 +114,6 @@ private:
 
 }
 
+#include <rtabmap/core/impl/OccupancyGrid.hpp>
+
 #endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -467,6 +467,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridDepthDecimation().c_str()));
 	RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridDepthDecimation().c_str()));
 	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridDepthDecimation().c_str()));
+	RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
+	RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
+	RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
 	RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridDepthDecimation().c_str()));
 	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
 	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
@@ -475,8 +478,9 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
 	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
 	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
-	RTABMAP_PARAM(Grid, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
-	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
+	RTABMAP_PARAM(Grid, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, ClusterRadius,           float,  0.1,     uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
 	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
 	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));

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

@@ -0,0 +1,162 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
+		const pcl::IndicesPtr & indicesIn,
+		const Transform & pose,
+		const cv::Point3f & viewPoint,
+		pcl::IndicesPtr & groundIndices,
+		pcl::IndicesPtr & obstaclesIndices,
+		pcl::IndicesPtr * flatObstacles) const
+{
+	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
+
+	// voxelize to grid cell size
+	cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
+	pcl::IndicesPtr indices(new std::vector<int>);
+	indices->resize(cloud->size());
+	for(unsigned int i=0; i<indices->size(); ++i)
+	{
+		indices->at(i) = i;
+	}
+
+	// add pose rotation without yaw
+	float roll, pitch, yaw;
+	pose.getEulerAngles(roll, pitch, yaw);
+	UDEBUG("node.getPose()=%s projMapFrame_=%d", pose.prettyPrint().c_str(), projMapFrame_?1:0);
+	cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0));
+
+	// filter footprint
+	if(footprintLength_ > 0.0f || footprintWidth_ > 0.0f || footprintHeight_ > 0.0f)
+	{
+		indices = util3d::cropBox(
+				cloud,
+				indices,
+				Eigen::Vector4f(
+						footprintLength_>0.0f?-footprintLength_/2.0f:std::numeric_limits<int>::min(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?-footprintWidth_/2.0f:std::numeric_limits<int>::min(),
+						0,
+						1),
+				Eigen::Vector4f(
+						footprintLength_>0.0f?footprintLength_/2.0f:std::numeric_limits<int>::max(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?footprintWidth_/2.0f:std::numeric_limits<int>::max(),
+						footprintHeight_>0.0f&&footprintLength_>0.0f&&footprintWidth_>0.0f?footprintHeight_:std::numeric_limits<int>::max(),
+						1),
+				Transform::getIdentity(),
+				true);
+	}
+
+	// filter ground/obstacles zone
+	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
+	{
+		indices = util3d::passThrough(cloud, indices, "z",
+				minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+				maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
+	}
+
+	if(indices->size())
+	{
+		if(normalsSegmentation_)
+		{
+			UDEBUG("normalKSearch=%d", normalKSearch_);
+			UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
+			UDEBUG("Cluster radius=%f", clusterRadius_);
+			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
+			UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
+			util3d::segmentObstaclesFromGround<PointT>(
+					cloud,
+					indices,
+					groundIndices,
+					obstaclesIndices,
+					normalKSearch_,
+					maxGroundAngle_,
+					clusterRadius_,
+					minClusterSize_,
+					flatObstaclesDetected_,
+					maxGroundHeight_,
+					flatObstacles,
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			//UWARN("Saving ground.pcd and obstacles.pcd");
+			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
+			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
+		}
+		else
+		{
+			UDEBUG("");
+			// passthrough filter
+			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
+			obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
+		}
+
+		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
+		{
+			UDEBUG("");
+			if(groundIndices->size())
+			{
+				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(obstaclesIndices->size())
+			{
+				obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(flatObstacles && (*flatObstacles)->size())
+			{
+				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+
+			if(groundIndices->empty() && obstaclesIndices->empty())
+			{
+				UWARN("Cloud (with %d points) is empty after noise "
+						"filtering. Occupancy grid cannot be "
+						"created.",
+						(int)cloud->size());
+
+			}
+		}
+	}
+	return cloud;
+}
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */

corelib/include/rtabmap/core/util3d_filtering.h

@@ -136,6 +136,33 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
 		float max,
 		bool negative = false);
 
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 pcl::IndicesPtr RTABMAP_EXP frustumFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/OccupancyGrid.cpp

@@ -27,8 +27,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/OccupancyGrid.h>
 #include <rtabmap/core/util3d.h>
-#include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
@@ -44,6 +42,9 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
 	cloudMinDepth_(Parameters::defaultGridDepthMin()),
 	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
+	footprintLength_(Parameters::defaultGridFootprintLength()),
+	footprintWidth_(Parameters::defaultGridFootprintWidth()),
+	footprintHeight_(Parameters::defaultGridFootprintHeight()),
 	scanDecimation_(Parameters::defaultGridScanDecimation()),
 	cellSize_(Parameters::defaultGridCellSize()),
 	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
@@ -51,6 +52,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
 	normalKSearch_(Parameters::defaultGridNormalK()),
 	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
+	clusterRadius_(Parameters::defaultGridClusterRadius()),
 	minClusterSize_(Parameters::defaultGridMinClusterSize()),
 	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
 	minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
@@ -74,6 +76,9 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
 	Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
 	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	Parameters::parse(parameters, Parameters::kGridFootprintLength(), footprintLength_);
+	Parameters::parse(parameters, Parameters::kGridFootprintWidth(), footprintWidth_);
+	Parameters::parse(parameters, Parameters::kGridFootprintHeight(), footprintHeight_);
 	Parameters::parse(parameters, Parameters::kGridScanDecimation(), scanDecimation_);
 	float cellSize = cellSize_;
 	if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
@@ -109,6 +114,8 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	{
 		maxGroundAngle_ *= M_PI/180.0f;
 	}
+	Parameters::parse(parameters, Parameters::kGridClusterRadius(), clusterRadius_);
+	UASSERT_MSG(clusterRadius_ > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridClusterRadius().c_str()).c_str());
 	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
 	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
 	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
@@ -175,7 +182,11 @@ void OccupancyGrid::setCellSize(float cellSize)
 	}
 }
 
-void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const
+void OccupancyGrid::createLocalMap(
+		const Signature & node,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		cv::Point3f & viewPoint) const
 {
 	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
 			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
@@ -211,7 +222,10 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 		}
 		else
 		{
-			UDEBUG("Depth image");
+			UDEBUG("Depth image : decimation=%d max=%f min=%f",
+					cloudDecimation_,
+					cloudMaxDepth_,
+					cloudMinDepth_);
 			cloud = util3d::cloudRGBFromSensorData(
 					node.sensorData(),
 					cloudDecimation_,
@@ -253,89 +267,18 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 
 		if(cloud->size())
 		{
-			// voxelize to grid cell size
-			cloud = util3d::voxelize(cloud, indices, cellSize_);
-			indices->resize(cloud->size());
-			for(unsigned int i=0; i<indices->size(); ++i)
+			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())
 			{
-				indices->at(i) = i;
-			}
-
-			// add pose rotation without yaw
-			float roll, pitch, yaw;
-			node.getPose().getEulerAngles(roll, pitch, yaw);
-			UDEBUG("node.getPose()=%s projMapFrame_=%d", node.getPose().prettyPrint().c_str(), projMapFrame_?1:0);
-			cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
-
-			if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
-			{
-				indices = util3d::passThrough(cloud, indices, "z",
-						minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
-						maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
-			}
-
-			pcl::IndicesPtr groundIndices, obstaclesIndices;
-
-			if(indices->size())
-			{
-				if(normalsSegmentation_)
-				{
-					UDEBUG("normalKSearch=%d", normalKSearch_);
-					UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
-					UDEBUG("Cluster radius=%f", cellSize_*2.0f);
-					UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
-					UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
-					util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
-							cloud,
-							indices,
-							groundIndices,
-							obstaclesIndices,
-							normalKSearch_,
-							maxGroundAngle_,
-							cellSize_*2.0f,
-							minClusterSize_,
-							flatObstaclesDetected_,
-							maxGroundHeight_,
-							0,
-							Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
-					UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
-					//UWARN("Saving ground.pcd and obstacles.pcd");
-					//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
-					//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
-				}
-				else
-				{
-					UDEBUG("");
-					// passthrough filter
-					groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
-					obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
-				}
-
-				UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
-
-				// Do radius filtering after voxel filtering ( a lot faster)
-				if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
-				{
-					UDEBUG("");
-					if(groundIndices->size())
-					{
-						groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
-					}
-					if(obstaclesIndices->size())
-					{
-						obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
-					}
-
-					if(groundIndices->empty() && obstaclesIndices->empty())
-					{
-						UWARN("Cloud (with %d points) is empty after noise "
-								"filtering. Occupancy grid of node %d cannot be "
-								"created.",
-								(int)cloud->size(), node.id());
-						return;
-					}
-				}
-
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 
@@ -359,6 +302,8 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 					}
 
 					// 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);

corelib/src/Parameters.cpp

@@ -186,27 +186,31 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 	return odomParameters;
 }
 
-ParametersMap Parameters::getDefaultParameters(const std::string & group)
+ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 {
 	rtabmap::ParametersMap parameters;
 	const rtabmap::ParametersMap & defaultParameters = rtabmap::Parameters::getDefaultParameters();
 	for(rtabmap::ParametersMap::const_iterator iter=defaultParameters.begin(); iter!=defaultParameters.end(); ++iter)
 	{
-		if(iter->first.compare(group) == 0)
+		UASSERT(uSplit(iter->first, '/').size()  == 2);
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare(groupIn) == 0)
 		{
 			parameters.insert(*iter);
 		}
 	}
-	UASSERT_MSG(parameters.size(), uFormat("No parameters found for group %s!", group.c_str()).c_str());
+	UASSERT_MSG(parameters.size(), uFormat("No parameters found for group %s!", groupIn.c_str()).c_str());
 	return parameters;
 }
 
-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn)
 {
 	ParametersMap output;
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		if(iter->first.compare(group) == 0)
+		UASSERT(uSplit(iter->first, '/').size()  == 2);
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare(groupIn) == 0)
 		{
 			output.insert(*iter);
 		}

corelib/src/util3d_filtering.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/frustum_culling.h>
 #include <pcl/filters/random_sample.h>
 #include <pcl/filters/passthrough.h>
+#include <pcl/filters/crop_box.h>
 
 #include <pcl/features/normal_3d_omp.h>
 
@@ -340,6 +341,101 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 	return output;
 }
 
+pcl::IndicesPtr cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform,
+		bool negative)
+{
+	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::CropBox<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
+	filter.setMin(min);
+	filter.setMax(max);
+	if(!transform.isNull() && !transform.isIdentity())
+	{
+		filter.setTransform(transform.toEigen3f());
+	}
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
+pcl::IndicesPtr cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform,
+		bool negative)
+{
+	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::CropBox<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
+	filter.setMin(min);
+	filter.setMax(max);
+	if(!transform.isNull() && !transform.isIdentity())
+	{
+		filter.setTransform(transform.toEigen3f());
+	}
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cropBox(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform,
+		bool negative)
+{
+	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::CropBox<pcl::PointXYZ> filter;
+	filter.setNegative(negative);
+	filter.setMin(min);
+	filter.setMax(max);
+	if(!transform.isNull() && !transform.isIdentity())
+	{
+		filter.setTransform(transform.toEigen3f());
+	}
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform,
+		bool negative)
+{
+	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::CropBox<pcl::PointXYZRGB> filter;
+	filter.setNegative(negative);
+	filter.setMin(min);
+	filter.setMax(max);
+	if(!transform.isNull() && !transform.isIdentity())
+	{
+		filter.setTransform(transform.toEigen3f());
+	}
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
 pcl::IndicesPtr frustumFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

guilib/src/DatabaseViewer.cpp

@@ -2338,8 +2338,6 @@ void DatabaseViewer::update(int value,
 				if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
 				{
 					view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
-					delete signatures.front();
-					signatures.clear();
 				}
 
 				Transform odomPose, g;
@@ -2421,6 +2419,14 @@ void DatabaseViewer::update(int value,
 						(!data.depthOrRightRaw().empty() ||
 						!data.laserScanRaw().empty()))
 				{
+					Transform pose = Transform::getIdentity();
+					if(signatures.size())
+					{
+						float x, y, z, roll, pitch, yaw;
+						(*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+						pose = Transform(0,0,z,roll,pitch,0);
+					}
+
 					view3D->removeAllFrustums();
 					view3D->removeCloud("0");
 					view3D->removeCloud("1");
@@ -2474,14 +2480,14 @@ void DatabaseViewer::update(int value,
 											ui_->checkBox_mesh_quad->isChecked(),
 											ui_->spinBox_mesh_triangleSize->value(),
 											viewpoint);
-									view3D->addCloudMesh("0", cloud, polygons);
+									view3D->addCloudMesh("0", cloud, polygons, pose);
 								}
 								else
 								{
-									view3D->addCloud("0", cloud);
+									view3D->addCloud("0", cloud, pose);
 								}
 							}
-							view3D->updateCameraFrustums(Transform::getIdentity(), data.cameraModels());
+							view3D->updateCameraFrustums(pose, data.cameraModels());
 						}
 						else
 						{
@@ -2489,8 +2495,8 @@ void DatabaseViewer::update(int value,
 							cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
 							if(cloud->size())
 							{
-								view3D->addCloud("0", cloud);
-								view3D->updateCameraFrustum(Transform::getIdentity(), data.stereoCameraModel());
+								view3D->addCloud("0", cloud, pose);
+								view3D->updateCameraFrustum(pose, data.stereoCameraModel());
 							}
 						}
 					}
@@ -2499,7 +2505,7 @@ void DatabaseViewer::update(int value,
 					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
 					if(scan->size())
 					{
-						view3D->addCloud("1", scan);
+						view3D->addCloud("1", scan, pose);
 					}
 
 					//add occupancy grid
@@ -2533,6 +2539,12 @@ void DatabaseViewer::update(int value,
 
 					view3D->update();
 				}
+				if(signatures.size())
+				{
+					UASSERT(signatures.front() != 0 && signatures.size() == 1);
+					delete signatures.front();
+					signatures.clear();
+				}
 			}
 
 			if(!img.isNull())

guilib/src/PreferencesDialog.cpp

@@ -775,6 +775,9 @@ 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->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());
 	_ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str());
 	_ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str());
 	_ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str());
@@ -782,6 +785,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str());
 	_ui->doubleSpinBox_grid_maxGroundHeight->setObjectName(Parameters::kGridMaxGroundHeight().c_str());
 	_ui->doubleSpinBox_grid_maxObstacleHeight->setObjectName(Parameters::kGridMaxObstacleHeight().c_str());
+	_ui->doubleSpinBox_grid_clusterRadius->setObjectName(Parameters::kGridClusterRadius().c_str());
 	_ui->spinBox_grid_minClusterSize->setObjectName(Parameters::kGridMinClusterSize().c_str());
 	_ui->doubleSpinBox_grid_minGroundHeight->setObjectName(Parameters::kGridMinGroundHeight().c_str());
 	_ui->spinBox_grid_noiseMinNeighbors->setObjectName(Parameters::kGridNoiseFilteringMinNeighbors().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>984</width>
-    <height>727</height>
+    <width>976</width>
+    <height>731</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>686</width>
-            <height>2278</height>
+            <y>-500</y>
+            <width>678</width>
+            <height>2417</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>15</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8105,7 +8105,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_331">
                         <property name="text">
                          <string>Laser scan decimation.</string>
@@ -8118,7 +8118,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="10" column="0">
                        <widget class="QSpinBox" name="spinBox_grid_scanDecimation">
                         <property name="minimum">
                          <number>1</number>
@@ -8131,6 +8131,102 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_333">
+                        <property name="text">
+                         <string>Footprint filtering length (0=disabled).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="1">
+                       <widget class="QLabel" name="label_334">
+                        <property name="text">
+                         <string>Footprint filtering width (0=disabled). Footprint length should be set.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintLength">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>10.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintWidth">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>10.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_335">
+                        <property name="text">
+                         <string>Footprint filtering height (0=disabled). Footprint length and width should be set.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintHeight">
+                        <property name="suffix">
+                         <string> m</string>
+                        </property>
+                        <property name="decimals">
+                         <number>2</number>
+                        </property>
+                        <property name="maximum">
+                         <double>10.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -8315,7 +8411,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
+                         <item row="3" column="0">
                           <widget class="QSpinBox" name="spinBox_grid_minClusterSize">
                            <property name="minimum">
                             <number>1</number>
@@ -8341,7 +8437,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_308">
                            <property name="text">
                             <string>Minimum cluster size to project the points. The distance between clusters is defined by 2*Resolution.</string>
@@ -8354,7 +8450,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_272">
                            <property name="text">
                             <string>Flat obstacles detected.</string>
@@ -8367,7 +8463,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="QCheckBox" name="checkBox_grid_flatObstaclesDetected">
                            <property name="text">
                             <string/>
@@ -8403,6 +8499,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_336">
+                           <property name="text">
+                            <string>Cluster radius.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clusterRadius">
+                           <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">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.100000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>