Point Cloud Library (PCL)
1.9.1
|
39 #ifndef PCL_FEATURES_IMPL_GRSD_H_
40 #define PCL_FEATURES_IMPL_GRSD_H_
42 #include <pcl/features/grsd.h>
44 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
int
46 double min_radius_plane,
47 double max_radius_noise,
48 double min_radius_cylinder,
49 double max_min_radius_diff)
51 if (min_radius > min_radius_plane)
53 else if (max_radius > min_radius_cylinder)
55 else if (min_radius < max_radius_noise)
57 else if (max_radius - min_radius < max_min_radius_diff)
64 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
70 PCL_ERROR (
"[pcl::%s::computeFeature] A voxel cell width needs to be set!\n", getClassName ().c_str ());
82 grid.
filter (*cloud_downsampled);
90 rsd.
setRadiusSearch (std::max (search_radius_, std::sqrt (3.0) * width_ / 2));
95 std::vector<int> types (radii->
points.size ());
96 for (
size_t idx = 0; idx < radii->
points.size (); ++idx)
97 types[idx] = getSimpleType (radii->
points[idx].r_min, radii->
points[idx].r_max);
100 Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1);
101 for (
size_t idx = 0; idx < cloud_downsampled->points.size (); ++idx)
103 int source_type = types[idx];
105 for (
unsigned id_n = 0; id_n < neighbors.size (); id_n++)
108 if (neighbors[id_n] == -1)
109 neighbor_type = NR_CLASS;
111 neighbor_type = types[neighbors[id_n]];
112 transition_matrix (source_type, neighbor_type)++;
120 for (
int i = 0; i < NR_CLASS + 1; i++)
121 for (
int j = i; j < NR_CLASS + 1; j++)
122 output.
points[0].histogram[nrf++] = transition_matrix (i, j) + transition_matrix (j, i);
125 #define PCL_INSTANTIATE_GRSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GRSDEstimation<T,NT,OutT>;
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
uint32_t width
The point cloud width (if organized as an image-structure).
PointCloudIn::Ptr PointCloudInPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
PointCloud represents the base class in PCL for storing collections of 3D points.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
RSDEstimation estimates the Radius-based Surface Descriptor (minimal and maximal radius of the local ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
uint32_t height
The point cloud height (if organized as an image-structure).
std::vector< int > getNeighborCentroidIndices(const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
static int getSimpleType(float min_radius, float max_radius, double min_radius_plane=0.100, double max_radius_noise=0.015, double min_radius_cylinder=0.175, double max_min_radius_diff=0.050)
Get the type of the local surface based on the min and max radius computed.
void computeFeature(PointCloudOut &output)
Estimate the Global Radius-based Surface Descriptor (GRSD) for a set of points given by <setInputClou...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.