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

util3d: added bool segmentFlatObstacles (default false) parameter to segmentObstaclesFromGround() method

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1980 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-11-11 21:34:14 +00:00
parent 7efc6aab2d
commit f9e25c2fa6
2 changed files with 32 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
corelib/include/rtabmap/core/impl/util3d.hpp
View File

@@ -146,7 +146,8 @@ void segmentObstaclesFromGround(
pcl::IndicesPtr & obstacles,
float normalRadiusSearch,
float groundNormalAngle,
int minClusterSize)
int minClusterSize,
bool segmentFlatObstacles)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
@@ -159,33 +160,40 @@ void segmentObstaclesFromGround(
normalRadiusSearch*2.0f,
Eigen::Vector4f(0,0,100,0));
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
if(segmentFlatObstacles)
{
if((int)i!=biggestFlatSurfaceIndex)
int biggestFlatSurfaceIndex;
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
cloud,
flatSurfaces,
normalRadiusSearch*2.0f,
minClusterSize,
std::numeric_limits<int>::max(),
&biggestFlatSurfaceIndex);
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
if((int)i!=biggestFlatSurfaceIndex)
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
Eigen::Vector4f centroid;
pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
}
else
{
ground = flatSurfaces;
}
if(ground->size() != cloud->size())
{