i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+27 -7
View File
@@ -27,7 +27,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle, float groundNormalAngle,
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles) bool segmentFlatObstacles,
float maxGroundHeight)
{ {
ground.reset(new std::vector<int>); ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>); obstacles.reset(new std::vector<int>);
@@ -56,10 +57,14 @@ void segmentObstaclesFromGround(
// cluster all surfaces for which the centroid is in the Z-range of the bigger surface // cluster all surfaces for which the centroid is in the Z-range of the bigger surface
if(clusteredFlatSurfaces.size())
{
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex); ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
Eigen::Vector4f min,max; Eigen::Vector4f min,max;
pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max); pcl::getMinMax3D(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
if(maxGroundHeight <= 0 || min[2] < maxGroundHeight)
{
for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i) for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
{ {
if((int)i!=biggestFlatSurfaceIndex) if((int)i!=biggestFlatSurfaceIndex)
@@ -74,6 +79,13 @@ void segmentObstaclesFromGround(
} }
} }
else else
{
// reject ground!
ground.reset(new std::vector<int>);
}
}
}
else
{ {
ground = flatSurfaces; ground = flatSurfaces;
} }
@@ -105,7 +117,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle, float groundNormalAngle,
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles) bool segmentFlatObstacles,
float maxGroundHeight)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>( segmentObstaclesFromGround<PointT>(
@@ -117,7 +130,8 @@ void segmentObstaclesFromGround(
groundNormalAngle, groundNormalAngle,
clusterRadius, clusterRadius,
minClusterSize, minClusterSize,
segmentFlatObstacles); segmentFlatObstacles,
maxGroundHeight);
} }
template<typename PointT> template<typename PointT>
@@ -128,7 +142,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles, cv::Mat & obstacles,
float cellSize, float cellSize,
float groundNormalAngle, float groundNormalAngle,
int minClusterSize) int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
{ {
if(cloud->size() == 0) if(cloud->size() == 0)
{ {
@@ -144,7 +160,9 @@ void occupancy2DFromCloud3D(
20, 20,
groundNormalAngle, groundNormalAngle,
cellSize*2.0f, cellSize*2.0f,
minClusterSize); minClusterSize,
segmentFlatObstacles,
maxGroundHeight);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(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, cv::Mat & obstacles,
float cellSize, float cellSize,
float groundNormalAngle, float groundNormalAngle,
int minClusterSize) int minClusterSize,
bool segmentFlatObstacles,
float maxGroundHeight)
{ {
pcl::IndicesPtr indices(new std::vector<int>); 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);
} }
} }
corelib/include/rtabmap/core/util3d_mapping.h
+10 -4
View File
@@ -90,7 +90,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle, float groundNormalAngle,
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false); bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT> template<typename PointT>
void segmentObstaclesFromGround( void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -100,7 +101,8 @@ void segmentObstaclesFromGround(
float groundNormalAngle, float groundNormalAngle,
float clusterRadius, float clusterRadius,
int minClusterSize, int minClusterSize,
bool segmentFlatObstacles = false); bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT> template<typename PointT>
void occupancy2DFromCloud3D( void occupancy2DFromCloud3D(
@@ -109,7 +111,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles, cv::Mat & obstacles,
float cellSize = 0.05f, float cellSize = 0.05f,
float groundNormalAngle = M_PI_4, float groundNormalAngle = M_PI_4,
int minClusterSize = 20); int minClusterSize = 20,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
template<typename PointT> template<typename PointT>
void occupancy2DFromCloud3D( void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -118,7 +122,9 @@ void occupancy2DFromCloud3D(
cv::Mat & obstacles, cv::Mat & obstacles,
float cellSize = 0.05f, float cellSize = 0.05f,
float groundNormalAngle = M_PI_4, float groundNormalAngle = M_PI_4,
int minClusterSize = 20); int minClusterSize = 20,
bool segmentFlatObstacles = false,
float maxGroundHeight = 0.0f);
} // namespace util3d } // namespace util3d
} // namespace rtabmap } // namespace rtabmap