API change: segmentObstaclesFromGround() and normalFiltering()

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>);

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);
 
 /**

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);