From 854a52c6a5f003741696ba9ac5ec2aec42cbc6d3 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 10 May 2021 14:45:35 -0400
Subject: [PATCH] Export CLI: added noise_radius and noise_k options to filter
 noise

---
 .../include/rtabmap/core/util3d_filtering.h   |  8 ++++
 corelib/src/util3d_filtering.cpp              | 10 +++++
 tools/Export/main.cpp                         | 40 +++++++++++++++++++
 3 files changed, 58 insertions(+)

diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h
index a530d65d..3834a2c9 100644
--- a/corelib/include/rtabmap/core/util3d_filtering.h
+++ b/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.
diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp
index dc9cf4b2..25318416 100644
--- a/corelib/src/util3d_filtering.cpp
+++ b/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(
diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp
index 1f6fa74b..00eee869 100644
--- a/tools/Export/main.cpp
+++ b/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())