i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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; std::set<int> flatObstaclesSet;
if(projObstaclesPub_.getNumSubscribers()) if(projObstaclesPub_.getNumSubscribers())
{ {
std::set<int> flatObstaclesSet(flatObstacles->begin(), flatObstacles->end()); flatObstaclesSet.insert(flatObstacles->begin(), flatObstacles->end());
} }
obstaclesCloud->resize(obstacles->size()); obstaclesCloud->resize(obstacles->size());
@@ -211,7 +211,7 @@ private:
for(unsigned int i=0; i<obstacles->size(); ++i) for(unsigned int i=0; i<obstacles->size(); ++i)
{ {
obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i)); obstaclesCloud->points[i] = originalCloud->at(obstacles->at(i));
if(flatObstaclesSet.size() && if(flatObstaclesSet.size() == 0 ||
flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end()) flatObstaclesSet.find(obstacles->at(i))==flatObstaclesSet.end())
{ {
obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i]; obstaclesCloudWithoutFlatSurfaces->points[oi] = obstaclesCloud->points[i];
@@ -220,6 +220,7 @@ private:
} }
} }
obstaclesCloudWithoutFlatSurfaces->resize(oi); obstaclesCloudWithoutFlatSurfaces->resize(oi);
if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0) if(obstaclesCloudWithoutFlatSurfaces->size() && projVoxelSize_ > 0.0)
{ {