From 8a26f42687b9ab4c4eef21fbe25279820aaa966b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 28 Nov 2024 20:58:40 -0800 Subject: [PATCH] 0.21.9: Grid's noise filtering only done on indices below Grid/RangeMax (when set). We can then keep very far points that would be filtered otherwise, which are useful for ray tracing. --- CMakeLists.txt | 2 +- .../rtabmap/core/impl/LocalMapMaker.hpp | 36 +++++ .../include/rtabmap/core/util3d_filtering.h | 46 ++++++ corelib/src/util3d_filtering.cpp | 148 ++++++++++++++++++ 4 files changed, 231 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index df11ddbb2b..b42fbf41e6 100644 --- 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 index a844d450b2..b7b0d066a3 100644 --- a/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp +++ b/corelib/include/rtabmap/core/impl/LocalMapMaker.hpp @@ -175,15 +175,51 @@ typename pcl::PointCloud::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 index 977a3b91d3..ef62c6d228 100644 --- 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::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax); +pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax); +pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax); +pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax); + +void RTABMAP_CORE_EXPORT rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices); +void RTABMAP_CORE_EXPORT rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices); +void RTABMAP_CORE_EXPORT rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices); +void RTABMAP_CORE_EXPORT rangeSplitFiltering( + const pcl::PointCloud::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 index 134cb6a235..dafe7dbedc 100644 --- a/corelib/src/util3d_filtering.cpp +++ b/corelib/src/util3d_filtering.cpp @@ -395,6 +395,154 @@ LaserScan rangeFiltering( return scan; } +template +pcl::IndicesPtr rangeFilteringImpl( + const typename pcl::PointCloud::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()); + 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; iempty()?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::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax) +{ + return rangeFilteringImpl(cloud, indices, rangeMin, rangeMax); +} +pcl::IndicesPtr rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax) +{ + return rangeFilteringImpl(cloud, indices, rangeMin, rangeMax); +} +pcl::IndicesPtr rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax) +{ + return rangeFilteringImpl(cloud, indices, rangeMin, rangeMax); +} +pcl::IndicesPtr rangeFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float rangeMin, + float rangeMax) +{ + return rangeFilteringImpl(cloud, indices, rangeMin, rangeMax); +} + +template +void rangeSplitFilteringImpl( + const typename pcl::PointCloud::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()); + farIndices.reset(new std::vector()); + closeIndices->reserve(size); + farIndices->reserve(size); + if(!cloud->empty()) + { + float rangeSqrd = range * range; + for(int i=0; iempty()?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::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices) +{ + rangeSplitFilteringImpl(cloud, indices, range, closeIndices, farIndices); +} +void rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices) +{ + rangeSplitFilteringImpl(cloud, indices, range, closeIndices, farIndices); +} +void rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices) +{ + rangeSplitFilteringImpl(cloud, indices, range, closeIndices, farIndices); +} +void rangeSplitFiltering( + const pcl::PointCloud::Ptr & cloud, + const pcl::IndicesPtr & indices, + float range, + pcl::IndicesPtr & closeIndices, + pcl::IndicesPtr & farIndices) +{ + rangeSplitFilteringImpl(cloud, indices, range, closeIndices, farIndices); +} + LaserScan downsample( const LaserScan & scan, int step)