Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

optimize obstacle segmentation for organized point clouds #1162

Draft
wants to merge 2 commits into
base: master
Choose a base branch
from
Draft

optimize obstacle segmentation for organized point clouds #1162

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d48e209
added fast normal estimation on obstacle segmentation
matlabbe Jun 2, 2023
0fcf562
latest update
matlabbe Jun 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/OccupancyGrid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,7 @@ class RTABMAP_CORE_EXPORT OccupancyGrid
bool normalsSegmentation_;
bool grid3D_;
bool groundIsObstacle_;
bool labelUndergroundObstaclesAsGround_;
float noiseFilteringRadius_;
int noiseFilteringMinNeighbors_;
bool scan2dUnknownSpaceFilled_;
Expand Down
1 change: 1 addition & 0 deletions corelib/include/rtabmap/core/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -760,6 +760,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Grid, 3D, bool, false, uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
#endif
RTABMAP_PARAM(Grid, GroundIsObstacle, bool, false, uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, UndergroundIsGround, bool, false, uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "Noise filtering radius (0=disabled). Done after segmentation.");
RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors.");
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
Expand Down
46 changes: 44 additions & 2 deletions corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
pcl::IndicesPtr & obstaclesIndices,
pcl::IndicesPtr * flatObstacles) const
{
UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size());

groundIndices.reset(new std::vector<int>);
obstaclesIndices.reset(new std::vector<int>);
if(flatObstacles)
Expand All @@ -54,6 +56,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::IndicesPtr indices(new std::vector<int>);

UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
if(preVoxelFiltering_)
{
// voxelize to grid cell size
Expand Down Expand Up @@ -127,6 +130,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size());
util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
Expand All @@ -140,8 +146,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
maxGroundHeight_,
flatObstacles,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
groundNormalsUp_);
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
groundNormalsUp_,
labelUndergroundObstaclesAsGround_);
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
Expand All @@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(

UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());

if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
{
// voxelize to grid cell size
typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
cloud.reset(new pcl::PointCloud<PointT>);
if(!groundIndices->empty())
{
*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
groundIndices->resize(cloud->size());
for(size_t i=0; i<groundIndices->size(); ++i)
{
groundIndices->at(i) = i;
}
}
if(!obstaclesIndices->empty())
{
int previousSize = cloud->size();
*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
obstaclesIndices->resize(cloud->size()-previousSize);
for(size_t i=0; i<obstaclesIndices->size(); ++i)
{
obstaclesIndices->at(i) = previousSize+i;
}
}
if(flatObstacles && !(*flatObstacles)->empty())
{
int previousSize = cloud->size();
*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
(*flatObstacles)->resize(cloud->size()-previousSize);
for(size_t i=0; i<(*flatObstacles)->size(); ++i)
{
(*flatObstacles)->at(i) = previousSize+i;
}
}
}

// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{
Expand Down
195 changes: 176 additions & 19 deletions corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,79 @@ typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
return output;
}

void clusterIndicesFloodfill(std::vector<int> & cluster,
float * visitedIndices,
int width,
int height,
float clusterRadius,
int currentIndex,
float previousHeight);

/**
* @brief Cluster indices of an organized cloud
*
* @tparam PointT
* @param cloud
* @param indices
* @param minClusterSize
* @param maxClusterSize
* @param biggestClusterIndex
* @return std::vector<pcl::IndicesPtr>
*/
template<typename PointT>
std::vector<pcl::IndicesPtr> clusterIndices(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const typename pcl::IndicesPtr & indices,
float clusterRadius,
int minClusterSize,
int maxClusterSize,
int * biggestClusterIndex)
{
std::vector<pcl::IndicesPtr> clusters;
if(cloud->empty())
{
return clusters;
}

UASSERT(cloud->isOrganized());

cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
float * ptr = visitedIndices.ptr<float>();
// init search image
for(size_t i = 0; i<indices->size(); ++i)
{
ptr[indices->at(i)] = cloud->at(indices->at(i)).z;
}

int largestCluster = -1;
int largestClusterSize = 0;
int sum = 0;
for(size_t i = 0; i<indices->size(); ++i)
{
if(ptr[indices->at(i)] != 0.0f)
{
pcl::IndicesPtr cluster(new pcl::Indices());
clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]);
if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize)
{
clusters.push_back(cluster);
if((int)cluster->size() > largestClusterSize)
{
sum+=cluster->size();
largestCluster = clusters.size()-1;
largestClusterSize = cluster->size();
}
}
}
}
if(biggestClusterIndex)
{
*biggestClusterIndex = largestCluster;
}

return clusters;
}

template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
Expand All @@ -64,7 +137,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
float groundNormalsUp,
bool labelUndergroundObstaclesAsGround)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
Expand All @@ -75,6 +149,8 @@ void segmentObstaclesFromGround(

if(cloud->size())
{
UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d",
cloud->size(), indices->size(), cloud->isOrganized()?1:0);
// Find the ground
pcl::IndicesPtr flatSurfaces = normalFiltering(
cloud,
Expand All @@ -84,22 +160,40 @@ void segmentObstaclesFromGround(
normalKSearch,
viewPoint,
groundNormalsUp);
UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
flatSurfaces->size(), indices->size(), cloud->size());

Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0);
if(segmentFlatObstacles && flatSurfaces->size())
{
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(

std::vector<pcl::IndicesPtr> clusteredFlatSurfaces;
if(cloud->isOrganized())
{
clusteredFlatSurfaces = clusterIndices<PointT>(
cloud,
flatSurfaces,
clusterRadius,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size());
}
else
{
clusteredFlatSurfaces = extractClusters(
cloud,
flatSurfaces,
clusterRadius,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
}

// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size())
{
Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
if(maxGroundHeight != 0.0f)
{
// Search for biggest surface under max ground height
Expand All @@ -125,17 +219,20 @@ void segmentObstaclesFromGround(
if(biggestFlatSurfaceIndex>=0)
{
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)",
ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
}

if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
if(!ground->empty() &&
(maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
{
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid(0,0,0,1);
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
Expand All @@ -145,9 +242,46 @@ void segmentObstaclesFromGround(
}
}
}

int groundRatio = 100*ground->size()/cloud->size();
int minGroundRatio = 10;
if(minGroundRatio != 0 && groundRatio<minGroundRatio)
{
if(labelUndergroundObstaclesAsGround && maxGroundHeight!=0.0f)
{
// just do passthrough (e.g. reflective floor)
UWARN("Failed normal segmentation (ground ratio=%d%%, ground height=%f), fallback to passThrough (label underground as ground is true).",
groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
// passthrough filter
ground = rtabmap::util3d::passThrough(cloud, indices, "z",
std::numeric_limits<int>::min(),
maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits<int>::max());

pcl::IndicesPtr notObstacles = ground;
if(indices->size())
{
notObstacles = util3d::extractIndices(cloud, indices, true);
notObstacles = util3d::concatenate(notObstacles, ground);
}
obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
return;
}
else
{
UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!",
groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
// reject ground!
ground.reset(new std::vector<int>);
if(flatObstacles)
{
*flatObstacles = flatSurfaces;
}
}
}
}
else
{
UWARN("Failed normal segmentation, could not detect the ground!");
// reject ground!
ground.reset(new std::vector<int>);
if(flatObstacles)
Expand All @@ -168,28 +302,49 @@ void segmentObstaclesFromGround(
pcl::IndicesPtr notObstacles = ground;
if(indices->size())
{
// This will ignore all points not in input indices for obstacles.
notObstacles = util3d::extractIndices(cloud, indices, true);
notObstacles = util3d::concatenate(notObstacles, ground);
}
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);

// If ground height is set, remove obstacles under it
if(maxGroundHeight != 0.0f)
// If ground height is set and if we label obstacles under it as ground
if(labelUndergroundObstaclesAsGround)
{
otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
float max = biggestSurfaceMax[2];
if(maxGroundHeight > 0)
{
max += maxGroundHeight;
}

pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
if(!underground->empty())
{
ground = util3d::concatenate(ground, underground);
notObstacles = util3d::concatenate(underground, notObstacles);
}
}

pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);

//Cluster remaining stuff (obstacles)
if(otherStuffIndices->size())
{
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
clusterRadius,
minClusterSize);

// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
if(minClusterSize>1)
{
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
clusterRadius,
minClusterSize);

// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
else
{
obstacles = otherStuffIndices;
}
}
}
}
Expand All @@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
float groundNormalsUp,
bool labelUndergroundObstaclesAsGround)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>(
Expand All @@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
maxGroundHeight,
flatObstacles,
viewPoint,
groundNormalsUp);
groundNormalsUp,
labelUndergroundObstaclesAsGround);
}

template<typename PointT>
Expand Down
Loading
Loading