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

obstacles_detection: Fixed proj_obstacles without flat obstacles

Browse Source
This commit is contained in:
Mathieu Labbe
2016-05-20 19:14:21 -04:00
parent 47547865cf
commit 486ecbbf62
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+3 -2
View File
@@ -201,7 +201,7 @@ private:
std::set<int> flatObstaclesSet;
if(projObstaclesPub_.getNumSubscribers())
{
std::set<int> flatObstaclesSet(flatObstacles->begin(), flatObstacles->end());
flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
}
obstaclesCloud->resize(obstacles->size());
@@ -211,7 +211,7 @@ private:
for(unsigned int i=0; i<obstacles->size(); ++i)
{
obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
if(flatObstaclesSet.size() &&
if(flatObstaclesSet.size() == 0 ||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
{
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
@@ -220,6 +220,7 @@ private:
}
}
obstaclesCloudWithoutFlatSurfaces->resize(oi);
if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
{