41 #ifndef PCL_FEATURES_IMPL_GFPFH_H_ 42 #define PCL_FEATURES_IMPL_GFPFH_H_ 44 #include <pcl/features/gfpfh.h> 45 #include <pcl/octree/octree_search.h> 46 #include <pcl/common/eigen.h> 52 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 62 output.
header = input_->header;
73 computeFeature (output);
79 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 91 std::vector< std::vector<int> > line_histograms;
92 for (
size_t i = 0; i < occupied_cells.
size (); ++i)
94 Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
96 for (
size_t j = i+1; j < occupied_cells.
size (); ++j)
99 Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
103 std::vector<int> histogram;
104 for (
size_t k = 0; k < intersected_cells.
size (); ++k)
106 std::vector<int> indices;
107 octree.
voxelSearch (intersected_cells[k], indices);
108 int label = emptyLabel ();
109 if (indices.size () != 0)
111 label = getDominantLabel (indices);
113 histogram.push_back (label);
116 line_histograms.push_back(histogram);
120 std::vector< std::vector<int> > transition_histograms;
121 computeTransitionHistograms (line_histograms, transition_histograms);
123 std::vector<float> distances;
124 computeDistancesToMean (transition_histograms, distances);
126 std::vector<float> gfpfh_histogram;
127 computeDistanceHistogram (distances, gfpfh_histogram);
133 std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.
points[0].histogram);
137 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 139 std::vector< std::vector<int> >& transition_histograms)
141 transition_histograms.resize (label_histograms.size ());
143 for (
size_t i = 0; i < label_histograms.size (); ++i)
145 transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0);
147 std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1);
148 for (
size_t k = 0; k < transitions.size (); ++k)
150 transitions[k].resize (getNumberOfClasses () + 1, 0);
153 for (
size_t k = 1; k < label_histograms[i].size (); ++k)
155 uint32_t first_class = label_histograms[i][k-1];
156 uint32_t second_class = label_histograms[i][k];
158 if (second_class < first_class)
159 std::swap (first_class, second_class);
161 transitions[first_class][second_class] += 1;
166 for (
int m = 0; m < static_cast<int> (transitions.size ()); ++m)
167 for (
int n = m; n < static_cast<int> (transitions[m].size ()); ++n)
169 transition_histograms[i][flat_index] = transitions[m][n];
173 assert (flat_index == static_cast<int> (transition_histograms[i].size ()));
178 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 180 std::vector<float>& distances)
182 distances.resize (transition_histograms.size ());
184 std::vector<float> mean_histogram;
185 computeMeanHistogram (transition_histograms, mean_histogram);
187 for (
size_t i = 0; i < transition_histograms.size (); ++i)
189 float d = computeHIKDistance (transition_histograms[i], mean_histogram);
195 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 197 std::vector<float>& histogram)
199 std::vector<float>::const_iterator min_it = std::min_element (distances.begin (), distances.end ());
200 assert (min_it != distances.end ());
201 const float min_value = *min_it;
203 std::vector<float>::const_iterator max_it = std::max_element (distances.begin (), distances.end ());
204 assert (max_it != distances.end());
205 const float max_value = *max_it;
207 histogram.resize (descriptorSize (), 0);
209 const float range = max_value - min_value;
210 const int max_bin = descriptorSize () - 1;
211 for (
size_t i = 0; i < distances.size (); ++i)
213 const float raw_bin =
static_cast<float> (descriptorSize ()) * (distances[i] - min_value) / range;
214 int bin = std::min (max_bin, static_cast<int> (floor (raw_bin)));
220 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 222 std::vector<float>& mean_histogram)
224 assert (histograms.size () > 0);
226 mean_histogram.resize (histograms[0].size (), 0);
227 for (
size_t i = 0; i < histograms.size (); ++i)
228 for (
size_t j = 0; j < histograms[i].size (); ++j)
229 mean_histogram[j] += static_cast<float> (histograms[i][j]);
231 for (
size_t i = 0; i < mean_histogram.size (); ++i)
232 mean_histogram[i] /= static_cast<float> (histograms.size ());
236 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
float 238 const std::vector<float>& mean_histogram)
240 assert (histogram.size () == mean_histogram.size ());
243 for (
size_t i = 0; i < histogram.size (); ++i)
244 norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
246 norm /=
static_cast<float> (histogram.size ());
251 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> boost::uint32_t
254 std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0);
255 for (
size_t i = 0; i < indices.size (); ++i)
257 uint32_t label = labels_->points[indices[i]].label;
261 std::vector<uint32_t>::const_iterator max_it;
262 max_it = std::max_element (counts.begin (), counts.end ());
263 if (max_it == counts.end ())
264 return (emptyLabel ());
266 return (static_cast<uint32_t> (max_it - counts.begin ()));
269 #define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>; 271 #endif // PCL_FEATURES_IMPL_GFPFH_H_
bool voxelSearch(const PointT &point, std::vector< int > &point_idx_data)
Search for neighbors within a voxel at given point.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
int getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f &origin, const Eigen::Vector3f &end, AlignedPointTVector &voxel_center_list, float precision=0.2)
Get a PointT vector of centers of voxels intersected by a line segment.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void computeDistancesToMean(const std::vector< std::vector< int > > &transition_histograms, std::vector< float > &distances)
Compute the distance of each transition histogram to the mean.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
void computeDistanceHistogram(const std::vector< float > &distances, std::vector< float > &histogram)
Compute the binned histogram of distance values.
void computeTransitionHistograms(const std::vector< std::vector< int > > &label_histograms, std::vector< std::vector< int > > &transition_histograms)
Compute the fixed-length histograms of transitions.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
void computeMeanHistogram(const std::vector< std::vector< int > > &histograms, std::vector< float > &mean_histogram)
Compute the mean histogram of the given set of histograms.
void computeFeature(PointCloudOut &output) override
Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by <setInputCloud ()...
void clear()
Removes all points in a cloud and sets the width and height to 0.
float computeHIKDistance(const std::vector< int > &histogram, const std::vector< float > &mean_histogram)
Return the Intersection Kernel distance between two histograms.
pcl::PCLHeader header
The point cloud header.
Octree pointcloud search class
uint32_t getDominantLabel(const std::vector< int > &indices)
Return the dominant label of a set of points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Feature represents the base feature class.