39#ifndef PCL_FEATURES_IMPL_GRSD_H_
40#define PCL_FEATURES_IMPL_GRSD_H_
42#include <pcl/features/grsd.h>
43#include <pcl/features/rsd.h>
45template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
int
47 double min_radius_plane,
48 double max_radius_noise,
49 double min_radius_cylinder,
50 double max_min_radius_diff)
52 if (min_radius > min_radius_plane)
54 if (max_radius > min_radius_cylinder)
56 if (min_radius < max_radius_noise)
58 if (max_radius - min_radius < max_min_radius_diff)
64template <
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 ());
71 output.width = output.height = 0;
82 grid.
filter (*cloud_downsampled);
90 rsd.
setRadiusSearch (std::max (search_radius_, std::sqrt (3.0) * width_ / 2));
95 std::vector<int> types (radii->size ());
96 std::transform(radii->points.cbegin (), radii->points.cend (), types.begin (),
97 [](
const auto& point) {
99 return GRSDEstimation<PointInT, PointNT, PointOutT>::getSimpleType(point.r_min, point.r_max); });
102 Eigen::MatrixXi transition_matrix = Eigen::MatrixXi::Zero (NR_CLASS + 1, NR_CLASS + 1);
103 for (std::size_t idx = 0; idx < cloud_downsampled->size (); ++idx)
105 const int source_type = types[idx];
107 for (
const int &neighbor : neighbors)
109 int neighbor_type = NR_CLASS;
111 neighbor_type = types[neighbor];
112 transition_matrix (source_type, neighbor_type)++;
118 output.height = output.width = 1;
120 for (
int i = 0; i < NR_CLASS + 1; i++)
121 for (
int j = i; j < NR_CLASS + 1; j++)
122 output[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>;
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
typename Feature< PointInT, PointOutT >::PointCloudIn PointCloudIn
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) override
Estimate the Global Radius-based Surface Descriptor (GRSD) for a set of points given by <setInputClou...
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
typename Feature< PointInT, PointOutT >::PointCloudInPtr PointCloudInPtr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
RSDEstimation estimates the Radius-based Surface Descriptor (minimal and maximal radius of the local ...
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
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...