diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 53fcce1c..529d79a5 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -23,8 +23,9 @@ void segmentObstaclesFromGround(
 		const typename pcl::IndicesPtr & indices,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles)
 {
@@ -39,7 +40,7 @@ void segmentObstaclesFromGround(
 				indices,
 				groundNormalAngle,
 				Eigen::Vector4f(0,0,1,0),
-				normalRadiusSearch*2.0f,
+				normalKSearch,
 				Eigen::Vector4f(0,0,100,0));
 
 		if(segmentFlatObstacles)
@@ -48,7 +49,7 @@ void segmentObstaclesFromGround(
 			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
 					cloud,
 					flatSurfaces,
-					normalRadiusSearch*2.0f,
+					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
@@ -86,7 +87,7 @@ void segmentObstaclesFromGround(
 			std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 					cloud,
 					otherStuffIndices,
-					normalRadiusSearch*2.0f,
+					clusterRadius,
 					minClusterSize);
 
 			// merge indices
@@ -100,8 +101,9 @@ void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles)
 {
@@ -111,8 +113,9 @@ void segmentObstaclesFromGround(
 			indices,
 			ground,
 			obstacles,
-			normalRadiusSearch,
+			normalKSearch,
 			groundNormalAngle,
+			clusterRadius,
 			minClusterSize,
 			segmentFlatObstacles);
 }
@@ -138,8 +141,9 @@ void occupancy2DFromCloud3D(
 			indices,
 			groundIndices,
 			obstaclesIndices,
-			cellSize,
+			20,
 			groundNormalAngle,
+			cellSize*2.0f,
 			minClusterSize);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h
index 04db359b..9426aec4 100644
--- a/corelib/include/rtabmap/core/util3d_filtering.h
+++ b/corelib/include/rtabmap/core/util3d_filtering.h
@@ -344,13 +344,13 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 
 /**
@@ -365,7 +365,7 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
  * @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 normalKSearch number of neighbor points 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.
  */
@@ -375,21 +375,21 @@ pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 pcl::IndicesPtr RTABMAP_EXP normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint);
 
 /**
diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h
index 3b43650d..5dbef1ca 100644
--- a/corelib/include/rtabmap/core/util3d_mapping.h
+++ b/corelib/include/rtabmap/core/util3d_mapping.h
@@ -86,8 +86,9 @@ void segmentObstaclesFromGround(
 		const pcl::IndicesPtr & indices,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false);
 template<typename PointT>
@@ -95,8 +96,9 @@ void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		pcl::IndicesPtr & ground,
 		pcl::IndicesPtr & obstacles,
-		float normalRadiusSearch,
+		int normalKSearch,
 		float groundNormalAngle,
+		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false);
 
diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp
index 1c24a0f1..9d71fb97 100644
--- a/corelib/src/util3d_filtering.cpp
+++ b/corelib/src/util3d_filtering.cpp
@@ -1219,21 +1219,21 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
 }
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+	return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
 }
 
 
@@ -1243,7 +1243,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
@@ -1274,7 +1274,7 @@ pcl::IndicesPtr normalFiltering(
 
 		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
 
-		ne.setRadiusSearch (radiusSearch);
+		ne.setKSearch(normalKSearch);
 		if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
 		{
 			ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1303,7 +1303,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());
@@ -1334,7 +1334,7 @@ pcl::IndicesPtr normalFiltering(
 
 		pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
 
-		ne.setRadiusSearch (radiusSearch);
+		ne.setKSearch (normalKSearch);
 		if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
 		{
 			ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
@@ -1363,7 +1363,7 @@ pcl::IndicesPtr normalFiltering(
 		const pcl::IndicesPtr & indices,
 		float angleMax,
 		const Eigen::Vector4f & normal,
-		float radiusSearch,
+		int normalKSearch,
 		const Eigen::Vector4f & viewpoint)
 {
 	pcl::IndicesPtr output(new std::vector<int>());