0.21.9: Grid's noise filtering only done on indices below Grid/RangeM…

…ax (when set). We can then keep very far points that would be filtered otherwise, which are useful for ray tracing.
introlab · Nov 29, 2024 · 8a26f42 · 8a26f42
1 parent 968b9f8
commit 8a26f42
Show file tree

Hide file tree

Showing 4 changed files with 231 additions and 1 deletion.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 8)
+SET(RTABMAP_PATCH_VERSION 9)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

diff --git a/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp b/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp
@@ -175,15 +175,51 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 					noiseFilteringMinNeighbors_);
 			if(groundIndices->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, groundIndices, rangeMax_, closeIndices, farIndices);
+					groundIndices = closeIndices;
+				}
 				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					groundIndices = rtabmap::util3d::concatenate(groundIndices, farIndices);
+				}
 			}
 			if(obstaclesIndices->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, obstaclesIndices, rangeMax_, closeIndices, farIndices);
+					obstaclesIndices = closeIndices;
+				}
 				obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					obstaclesIndices = rtabmap::util3d::concatenate(obstaclesIndices, farIndices);
+				}
 			}
 			if(flatObstacles && (*flatObstacles)->size())
 			{
+				pcl::IndicesPtr farIndices;
+				if(rangeMax_!=0)
+				{
+					// Don't filter points farther than maximum range, in case we want to ray trace empty space
+					pcl::IndicesPtr closeIndices;
+					rtabmap::util3d::rangeSplitFiltering(cloud, *flatObstacles, rangeMax_, closeIndices, farIndices);
+					*flatObstacles = closeIndices;
+				}
 				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				if(farIndices.get())
+				{
+					*flatObstacles = rtabmap::util3d::concatenate(*flatObstacles, farIndices);
+				}
 			}
 			UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
 					groundIndices->size(),

diff --git a/corelib/include/rtabmap/core/util3d_filtering.h b/corelib/include/rtabmap/core/util3d_filtering.h
@@ -73,6 +73,52 @@ LaserScan RTABMAP_CORE_EXPORT rangeFiltering(
 		float rangeMin,
 		float rangeMax);
 
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax);
+
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range, 
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+void RTABMAP_CORE_EXPORT rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices);
+
 LaserScan RTABMAP_CORE_EXPORT downsample(
 		const LaserScan & cloud,
 		int step);

diff --git a/corelib/src/util3d_filtering.cpp b/corelib/src/util3d_filtering.cpp
@@ -395,6 +395,154 @@ LaserScan rangeFiltering(
 	return scan;
 }
 
+template<typename PointT>
+pcl::IndicesPtr rangeFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	UASSERT(rangeMin >=0.0f && rangeMax>=0.0f);
+	int size = indices->empty()?cloud->size():indices->size();
+	pcl::IndicesPtr output(new std::vector<int>());
+	output->reserve(size);
+	if(!cloud->empty())
+	{
+		if(rangeMin > 0.0f || rangeMax > 0.0f)
+		{
+			float rangeMinSqrd = rangeMin * rangeMin;
+			float rangeMaxSqrd = rangeMax * rangeMax;
+			for(int i=0; i<size; ++i)
+			{
+				int index = indices->empty()?i:indices->at(i);
+				const PointT & pt = cloud->at(index);
+				float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+				if(rangeMin > 0.0f && r < rangeMinSqrd)
+				{
+					continue;
+				}
+				if(rangeMax > 0.0f && r > rangeMaxSqrd)
+				{
+					continue;
+				}
+
+				output->push_back(index);
+			}
+		}
+		else
+		{
+			*output = *indices;
+		}
+	}
+
+	return output;
+}
+
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZ>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZRGB>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointNormal>(cloud, indices, rangeMin, rangeMax);
+}
+pcl::IndicesPtr rangeFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float rangeMin,
+		float rangeMax)
+{
+	return rangeFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, rangeMin, rangeMax);
+}
+
+template<typename PointT>
+void rangeSplitFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	int size = indices->empty()?cloud->size():indices->size();
+	closeIndices.reset(new std::vector<int>());
+	farIndices.reset(new std::vector<int>());
+	closeIndices->reserve(size);
+	farIndices->reserve(size);
+	if(!cloud->empty())
+	{
+		float rangeSqrd = range * range;
+		for(int i=0; i<size; ++i)
+		{
+			int index = indices->empty()?i:indices->at(i);
+			const PointT & pt = cloud->at(index);
+			float r = pt.x*pt.x + pt.y*pt.y + pt.z*pt.z;
+
+			if(r < rangeSqrd)
+			{
+				closeIndices->push_back(index);
+			}
+			else
+			{
+				farIndices->push_back(index);
+			}
+		}
+	}
+}
+
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZ>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZRGB>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointNormal>(cloud, indices, range, closeIndices, farIndices);
+}
+void rangeSplitFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float range,
+		pcl::IndicesPtr & closeIndices,
+		pcl::IndicesPtr & farIndices)
+{
+	rangeSplitFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, range, closeIndices, farIndices);
+}
+
 LaserScan downsample(
 		const LaserScan & scan,
 		int step)