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

Segment ground/obstacles: added max ground height parameter

Browse Source
This commit is contained in:
Mathieu Labbe
2016-04-15 17:41:40 -04:00
parent 8bb5f0d905
commit 4cc1c66f09
2 changed files with 47 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -27,7 +27,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle,
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles)
bool segmentFlatObstacles,
float maxGroundHeight)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
@@ -56,21 +57,32 @@ void segmentObstaclesFromGround(
// 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(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
if(clusteredFlatSurfaces.size())
{
if((int)i!=biggestFlatSurfaceIndex)
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
if((int)i!=biggestFlatSurfaceIndex)
{
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(centroid[2] >= min[2] && centroid[2] <= max[2])
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
}
}
}
else
{
// reject ground!
ground.reset(new std::vector<int>);
}
}
}
else
@@ -105,7 +117,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle,
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles)
bool segmentFlatObstacles,
float maxGroundHeight)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>(
@@ -117,7 +130,8 @@ void segmentObstaclesFromGround(
groundNormalAngle,
clusterRadius,
minClusterSize,
segmentFlatObstacles);
segmentFlatObstacles,
maxGroundHeight);
}
template<typename PointT>
@@ -128,7 +142,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
{
if(cloud->size() == 0)
{
@@ -144,7 +160,9 @@ void occupancy2DFromCloud3D(
20,
groundNormalAngle,
cellSize*2.0f,
minClusterSize);
minClusterSize,
segmentFlatObstacles,
maxGroundHeight);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -197,10 +215,12 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
{
pcl::IndicesPtr indices(new std::vector<int>);
occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize);
occupancy2DFromCloud3D<PointT>(cloud, indices, ground, obstacles, cellSize, groundNormalAngle, minClusterSize, segmentFlatObstacles, maxGroundHeight);
}
}

14
corelib/include/rtabmap/core/util3d_mapping.h
View File

@@ -90,7 +90,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle,
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles = false);
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -100,7 +101,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle,
float clusterRadius,
int minClusterSize,
bool segmentFlatObstacles = false);
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT>
void occupancy2DFromCloud3D(
@@ -109,7 +111,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles,
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
int minClusterSize = 20,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT>
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -118,7 +122,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles,
float cellSize = 0.05f,
float groundNormalAngle = M_PI_4,
int minClusterSize = 20);
int minClusterSize = 20,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
} // namespace util3d
} // namespace rtabmap