i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing ground segmented as obstacle when grid noise filtering is enabled and not obstacles under max range (fix https://github.com/introlab/rtabmap_ros/issues/1444) (#1752)

Browse Source
This commit is contained in:
matlabbe
2026-08-23 22:45:00 -07:00
committed by GitHub
parent 9c1e117384
commit 94c93ac9b1
3 changed files with 514 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/LocalGridMaker.h
+20
View File
@@ -148,6 +148,26 @@ public:
cv::Mat & emptyCells,
cv::Point3f & viewPointInOut) const;
private:
/**
* @brief Radius filters the points closer than @p maxRange, keeping the farther
* points as-is (in case we want to ray trace empty space beyond max range).
* @param cloud Input cloud.
* @param indices Subset of @p cloud to filter. Asserted to be non-null and non-empty
* (@ref util3d::radiusFiltering() would filter the whole cloud otherwise).
* @param maxRange Range (m) above which points are not filtered. 0 means no limit.
* @param radiusSearch Radius (m) used for the noise filtering.
* @param minNeighborsInRadius Minimum neighbors required inside @p radiusSearch.
* @return Indices of the points that passed the filtering, plus those beyond @p maxRange.
*/
template<typename PointT>
static pcl::IndicesPtr noiseFilteringWithMaxRange(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxRange,
float radiusSearch,
int minNeighborsInRadius);
protected:
ParametersMap parameters_;
corelib/include/rtabmap/core/impl/LocalMapMaker.hpp
+33 -39
View File
@@ -29,11 +29,41 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
template<typename PointT>
pcl::IndicesPtr LocalGridMaker::noiseFilteringWithMaxRange(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxRange,
float radiusSearch,
int minNeighborsInRadius)
{
// util3d::radiusFiltering() would filter the whole cloud if the indices are empty
UASSERT(indices.get() && !indices->empty());
pcl::IndicesPtr closeIndices = indices;
pcl::IndicesPtr farIndices;
if(maxRange != 0.0f)
{
// Don't filter points farther than maximum range, in case we want to ray trace empty space
util3d::rangeSplitFiltering(cloud, indices, maxRange, closeIndices, farIndices);
}
if(closeIndices->size())
{
// Note: radiusFiltering() would filter the whole cloud if indices are empty
closeIndices = util3d::radiusFiltering(cloud, closeIndices, radiusSearch, minNeighborsInRadius);
}
if(farIndices.get())
{
closeIndices = util3d::concatenate(closeIndices, farIndices);
}
return closeIndices;
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
@@ -187,51 +217,15 @@ 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);
}
groundIndices = noiseFilteringWithMaxRange<PointT>(cloud, groundIndices, rangeMax_, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
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);
}
obstaclesIndices = noiseFilteringWithMaxRange<PointT>(cloud, obstaclesIndices, rangeMax_, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
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);
}
*flatObstacles = noiseFilteringWithMaxRange<PointT>(cloud, *flatObstacles, rangeMax_, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
}
UDEBUG("Radius filtering end (%ld ground %ld obstacles)",
groundIndices->size(),