i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
matlabbe
2024-11-28 20:58:40 -08:00
parent 968b9f8609
commit 8a26f42687
4 changed files with 231 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
corelib/include/rtabmap/core/impl/LocalMapMaker.hpp
View File

@@ -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(),

46
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -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);