28 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ 29 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ 37 template<
typename Po
intT>
39 const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
40 const pcl::IndicesPtr & indicesIn,
42 const cv::Point3f & viewPoint,
43 pcl::IndicesPtr & groundIndices,
44 pcl::IndicesPtr & obstaclesIndices,
45 pcl::IndicesPtr * flatObstacles)
const 47 groundIndices.reset(
new std::vector<int>);
48 obstaclesIndices.reset(
new std::vector<int>);
51 flatObstacles->reset(
new std::vector<int>);
54 typename pcl::PointCloud<PointT>::Ptr cloud(
new pcl::PointCloud<PointT>);
55 pcl::IndicesPtr indices(
new std::vector<int>);
62 indices->resize(cloud->size());
63 for(
unsigned int i=0; i<indices->size(); ++i)
71 if(indicesIn->empty() && cloud->is_dense)
73 indices->resize(cloud->size());
74 for(
unsigned int i=0; i<indices->size(); ++i)
117 UDEBUG(
"indices after max obstacles height filtering = %d", (
int)indices->size());
129 util3d::segmentObstaclesFromGround<PointT>(
141 Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(
projMapFrame_?pose.
z():0), 1));
155 pcl::IndicesPtr notObstacles = groundIndices;
164 UDEBUG(
"groundIndices=%d obstaclesIndices=%d", (
int)groundIndices->size(), (int)obstaclesIndices->size());
170 if(groundIndices->size())
174 if(obstaclesIndices->size())
178 if(flatObstacles && (*flatObstacles)->size())
183 if(groundIndices->empty() && obstaclesIndices->empty())
185 UWARN(
"Cloud (with %d points) is empty after noise " 186 "filtering. Occupancy grid cannot be " GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
pcl::IndicesPtr RTABMAP_EXP passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
pcl::IndicesPtr RTABMAP_EXP extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
pcl::PointCloud< PointT >::Ptr segmentCloud(const typename pcl::PointCloud< PointT >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &pose, const cv::Point3f &viewPoint, pcl::IndicesPtr &groundIndices, pcl::IndicesPtr &obstaclesIndices, pcl::IndicesPtr *flatObstacles=0) const
bool flatObstaclesDetected_
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
bool normalsSegmentation_
pcl::IndicesPtr RTABMAP_EXP cropBox(const pcl::PCLPointCloud2::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
ULogger class and convenient macros.
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
float noiseFilteringRadius_
pcl::IndicesPtr RTABMAP_EXP concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
int noiseFilteringMinNeighbors_