Go to the documentation of this file.
28 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_HPP_
29 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_LOCALMAP_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>);
71 if(indicesIn->empty() && cloud->is_dense)
117 UDEBUG(
"indices after max obstacles height filtering = %d", (
int)
indices->size());
130 util3d::segmentObstaclesFromGround<PointT>(
142 Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(
projMapFrame_?pose.
z():0), 1),
157 pcl::IndicesPtr notObstacles = groundIndices;
166 UDEBUG(
"groundIndices=%d obstaclesIndices=%d", (
int)groundIndices->size(), (
int)obstaclesIndices->size());
171 UDEBUG(
"Radius filtering (%ld ground %ld obstacles, radius=%f k=%d)",
172 groundIndices->size(),
173 obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0),
176 if(groundIndices->size())
180 if(obstaclesIndices->size())
184 if(flatObstacles && (*flatObstacles)->size())
188 UDEBUG(
"Radius filtering end (%ld ground %ld obstacles)",
189 groundIndices->size(),
190 obstaclesIndices->size()+(flatObstacles?(*flatObstacles)->size():0));
192 if(groundIndices->empty() && obstaclesIndices->empty())
194 UWARN(
"Cloud (with %d points) is empty after noise "
195 "filtering. Occupancy grid cannot be "
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
float noiseFilteringRadius_
pcl::IndicesPtr RTABMAP_CORE_EXPORT 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)
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
pcl::IndicesPtr RTABMAP_CORE_EXPORT passThrough(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const std::string &axis, float min, float max, bool negative=false)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
pcl::IndicesPtr RTABMAP_CORE_EXPORT concatenate(const std::vector< pcl::IndicesPtr > &indices)
Concatenate a vector of indices to a single vector.
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
int noiseFilteringMinNeighbors_
bool normalsSegmentation_
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
pcl::IndicesPtr RTABMAP_CORE_EXPORT extractIndices(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, bool negative)
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
ULogger class and convenient macros.
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_
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
pcl::IndicesPtr RTABMAP_CORE_EXPORT radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11