Export CLI: added noise_radius and noise_k options to filter noise

corelib/include/rtabmap/core/util3d_filtering.h

@@ -397,6 +397,14 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		float radiusSearch,
 		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
+pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
 
 /**
  * @brief Wrapper of the pcl::RadiusOutlierRemoval class.

corelib/src/util3d_filtering.cpp

@@ -993,6 +993,16 @@ pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZRGBNormal>::P
 	pcl::IndicesPtr indices(new std::vector<int>);
 	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
 }
+pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZI>::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::PointXYZINormal>::Ptr & cloud, float radiusSearch, int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
+}
 
 template<typename PointT>
 pcl::IndicesPtr radiusFilteringImpl(

tools/Export/main.cpp

@@ -93,6 +93,8 @@ void showUsage()
 			"    --max_range     #     Maximum range of the created clouds (default 4 m, 0 m with --scan).\n"
 			"    --decimation    #     Depth image decimation before creating the clouds (default 4, 1 with --scan).\n"
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
+			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
+			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled)."
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
@@ -132,6 +134,8 @@ int main(int argc, char * argv[])
 	int decimation = -1;
 	float maxRange = -1.0f;
 	float voxelSize = -1.0f;
+	float noiseRadius = 0.0f;
+	int noiseMinNeighbors = 5;
 	int textureSize = 4096;
 	int textureCount = 1;
 	int textureRange = 0;
@@ -377,6 +381,30 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--noise_radius") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				noiseRadius = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--noise_k") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				noiseMinNeighbors = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--color_radius") == 0)
 		{
 			++i;
@@ -673,10 +701,18 @@ int main(int argc, char * argv[])
 			if(scan.hasRGB())
 			{
 				cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+				if(noiseRadius>0.0f && noiseMinNeighbors>0)
+				{
+					indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors);
+				}
 			}
 			else
 			{
 				cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
+				if(noiseRadius>0.0f && noiseMinNeighbors>0)
+				{
+					indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors);
+				}
 			}
 		}
 		else
@@ -688,6 +724,10 @@ int main(int argc, char * argv[])
 					maxRange,        // maximum depth of the cloud
 					0.0f,
 					indices.get());
+			if(noiseRadius>0.0f && noiseMinNeighbors>0)
+			{
+				indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
+			}
 		}
 
 		if(exportImages && !rgb.empty())