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

Added some empty cloud verifications

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-27 17:19:21 -04:00
parent fb85434273
commit d53f4d08ce
1 changed files with 91 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/impl/util3d.hpp
+9 -1
View File
@@ -152,6 +152,8 @@ void segmentObstaclesFromGround(
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
if(cloud->size())
{
// Find the ground
pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
cloud,
@@ -210,6 +212,7 @@ void segmentObstaclesFromGround(
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
}
}
template<typename PointT>
@@ -302,6 +305,10 @@ pcl::IndicesPtr normalFiltering(
float radiusSearch,
const Eigen::Vector4f & viewpoint)
{
pcl::IndicesPtr output(new std::vector<int>());
if(cloud->size())
{
typedef typename pcl::search::KdTree<PointT> KdTree;
typedef typename KdTree::Ptr KdTreePtr;
@@ -334,7 +341,7 @@ pcl::IndicesPtr normalFiltering(
ne.compute (*cloud_normals);
pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
output->resize(cloud_normals->size());
int oi = 0; // output iterator
Eigen::Vector3f n(normal[0], normal[1], normal[2]);
for(unsigned int i=0; i<cloud_normals->size(); ++i)
@@ -347,6 +354,7 @@ pcl::IndicesPtr normalFiltering(
}
}
output->resize(oi);
}
return output;
}