Point Cloud Library (PCL) 1.13.0
gfpfh.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: gfpfh.hpp 2218 2011-08-25 20:27:15Z rusu $
38 *
39 */
40
41#ifndef PCL_FEATURES_IMPL_GFPFH_H_
42#define PCL_FEATURES_IMPL_GFPFH_H_
43
44#include <pcl/features/gfpfh.h>
45#include <pcl/octree/octree_search.h>
46#include <Eigen/Core> // for Vector3f
47
48#include <algorithm>
49#include <fstream>
50
51//////////////////////////////////////////////////////////////////////////////////////////////
52template<typename PointInT, typename PointNT, typename PointOutT> void
54{
56 {
57 output.width = output.height = 0;
58 output.clear ();
59 return;
60 }
61 // Copy the header
62 output.header = input_->header;
63
64 // Resize the output dataset
65 // Important! We should only allocate precisely how many elements we will need, otherwise
66 // we risk at pre-allocating too much memory which could lead to bad_alloc
67 // (see http://dev.pointclouds.org/issues/657)
68 output.width = output.height = 1;
69 output.is_dense = input_->is_dense;
70 output.resize (1);
71
72 // Perform the actual feature computation
73 computeFeature (output);
74
76}
77
78//////////////////////////////////////////////////////////////////////////////////////////////
79template <typename PointInT, typename PointNT, typename PointOutT> void
81{
82 pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
83 octree.setInputCloud (input_);
85
86 typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
87 octree.getOccupiedVoxelCenters (occupied_cells);
88
89 // Determine the voxels crosses along the line segments
90 // formed by every pair of occupied cells.
91 std::vector< std::vector<int> > line_histograms;
92 for (std::size_t i = 0; i < occupied_cells.size (); ++i)
93 {
94 Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
95
96 for (std::size_t j = i+1; j < occupied_cells.size (); ++j)
97 {
98 typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
99 Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
100 octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
101
102 // Intersected cells are ordered from closest to furthest w.r.t. the origin.
103 std::vector<int> histogram;
104 for (std::size_t k = 0; k < intersected_cells.size (); ++k)
105 {
106 pcl::Indices indices;
107 octree.voxelSearch (intersected_cells[k], indices);
108 int label = emptyLabel ();
109 if (!indices.empty ())
110 {
111 label = getDominantLabel (indices);
112 }
113 histogram.push_back (label);
114 }
115
116 line_histograms.push_back(histogram);
117 }
118 }
119
120 std::vector< std::vector<int> > transition_histograms;
121 computeTransitionHistograms (line_histograms, transition_histograms);
122
123 std::vector<float> distances;
124 computeDistancesToMean (transition_histograms, distances);
125
126 std::vector<float> gfpfh_histogram;
127 computeDistanceHistogram (distances, gfpfh_histogram);
128
129 output.clear ();
130 output.width = 1;
131 output.height = 1;
132 output.resize (1);
133 std::copy (gfpfh_histogram.cbegin (), gfpfh_histogram.cend (), output[0].histogram);
134}
135
136//////////////////////////////////////////////////////////////////////////////////////////////
137template <typename PointInT, typename PointNT, typename PointOutT> void
138pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeTransitionHistograms (const std::vector< std::vector<int> >& label_histograms,
139 std::vector< std::vector<int> >& transition_histograms)
140{
141 transition_histograms.resize (label_histograms.size ());
142
143 for (std::size_t i = 0; i < label_histograms.size (); ++i)
144 {
145 transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0);
146
147 std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1);
148 for (auto &transition : transitions)
149 {
150 transition.resize (getNumberOfClasses () + 1, 0);
151 }
152
153 for (std::size_t k = 1; k < label_histograms[i].size (); ++k)
154 {
155 std::uint32_t first_class = label_histograms[i][k-1];
156 std::uint32_t second_class = label_histograms[i][k];
157 // Order has no influence.
158 if (second_class < first_class)
159 std::swap (first_class, second_class);
160
161 transitions[first_class][second_class] += 1;
162 }
163
164 // Build a one-dimension histogram out of it.
165 std::size_t flat_index = 0;
166 for (std::size_t m = 0; m < transitions.size (); ++m)
167 for (std::size_t n = m; n < transitions[m].size (); ++n)
168 {
169 transition_histograms[i][flat_index] = transitions[m][n];
170 ++flat_index;
171 }
172
173 assert (flat_index == transition_histograms[i].size ());
174 }
175}
176
177//////////////////////////////////////////////////////////////////////////////////////////////
178template <typename PointInT, typename PointNT, typename PointOutT> void
179pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistancesToMean (const std::vector< std::vector<int> >& transition_histograms,
180 std::vector<float>& distances)
181{
182 distances.resize (transition_histograms.size ());
183
184 std::vector<float> mean_histogram;
185 computeMeanHistogram (transition_histograms, mean_histogram);
186
187 for (std::size_t i = 0; i < transition_histograms.size (); ++i)
188 {
189 float d = computeHIKDistance (transition_histograms[i], mean_histogram);
190 distances[i] = d;
191 }
192}
193
194//////////////////////////////////////////////////////////////////////////////////////////////
195template <typename PointInT, typename PointNT, typename PointOutT> void
197 std::vector<float>& histogram)
198{
199 std::vector<float>::const_iterator min_it, max_it;
200 std::tie (min_it, max_it) = std::minmax_element (distances.cbegin (), distances.cend ());
201 assert (min_it != distances.cend ());
202 assert (max_it != distances.cend ());
203
204 const float min_value = *min_it;
205 const float max_value = *max_it;
206
207 histogram.resize (descriptorSize (), 0);
208
209 const float range = max_value - min_value;
210
211 using binSizeT = decltype(descriptorSize());
212 const binSizeT max_bin = descriptorSize () - 1;
213 for (const float &distance : distances)
214 {
215 const auto raw_bin = descriptorSize () * (distance - min_value) / range;
216 const auto bin = std::min<binSizeT> (max_bin, static_cast<binSizeT> (std::floor (raw_bin)));
217 histogram[bin] += 1;
218 }
219}
220
221//////////////////////////////////////////////////////////////////////////////////////////////
222template <typename PointInT, typename PointNT, typename PointOutT> void
223pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeMeanHistogram (const std::vector< std::vector<int> >& histograms,
224 std::vector<float>& mean_histogram)
225{
226 assert (histograms.size () > 0);
227
228 mean_histogram.resize (histograms[0].size (), 0);
229 for (const auto &histogram : histograms)
230 for (std::size_t j = 0; j < histogram.size (); ++j)
231 mean_histogram[j] += static_cast<float> (histogram[j]);
232
233 for (float &i : mean_histogram)
234 i /= static_cast<float> (histograms.size ());
235}
236
237//////////////////////////////////////////////////////////////////////////////////////////////
238template <typename PointInT, typename PointNT, typename PointOutT> float
240 const std::vector<float>& mean_histogram)
241{
242 assert (histogram.size () == mean_histogram.size ());
243
244 float norm = 0.f;
245 for (std::size_t i = 0; i < histogram.size (); ++i)
246 norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
247
248 norm /= static_cast<float> (histogram.size ());
249 return (norm);
250}
251
252//////////////////////////////////////////////////////////////////////////////////////////////
253template <typename PointInT, typename PointNT, typename PointOutT> std::uint32_t
255{
256 std::vector<std::uint32_t> counts (getNumberOfClasses () + 1, 0);
257 for (const auto &nn_index : indices)
258 {
259 std::uint32_t label = (*labels_)[nn_index].label;
260 counts[label] += 1;
261 }
262
263 const auto max_it = std::max_element (counts.cbegin (), counts.cend ());
264 if (max_it == counts.end ())
265 return (emptyLabel ());
266
267 return std::distance(counts.cbegin (), max_it);
268}
269
270#define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>;
271
272#endif // PCL_FEATURES_IMPL_GFPFH_H_
Feature represents the base feature class.
Definition: feature.h:107
void computeDistancesToMean(const std::vector< std::vector< int > > &transition_histograms, std::vector< float > &distances)
Compute the distance of each transition histogram to the mean.
Definition: gfpfh.hpp:179
void computeTransitionHistograms(const std::vector< std::vector< int > > &label_histograms, std::vector< std::vector< int > > &transition_histograms)
Compute the fixed-length histograms of transitions.
Definition: gfpfh.hpp:138
std::uint32_t getDominantLabel(const pcl::Indices &indices)
Return the dominant label of a set of points.
Definition: gfpfh.hpp:254
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: gfpfh.hpp:53
void computeMeanHistogram(const std::vector< std::vector< int > > &histograms, std::vector< float > &mean_histogram)
Compute the mean histogram of the given set of histograms.
Definition: gfpfh.hpp:223
float computeHIKDistance(const std::vector< int > &histogram, const std::vector< float > &mean_histogram)
Return the Intersection Kernel distance between two histograms.
Definition: gfpfh.hpp:239
void computeDistanceHistogram(const std::vector< float > &distances, std::vector< float > &histogram)
Compute the binned histogram of distance values.
Definition: gfpfh.hpp:196
void computeFeature(PointCloudOut &output) override
Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by <setInputCloud ()...
Definition: gfpfh.hpp:80
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
std::size_t size() const
Definition: point_cloud.h:443
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:411
uindex_t getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
uindex_t 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.
Octree pointcloud search class
Definition: octree_search.h:58
bool voxelSearch(const PointT &point, Indices &point_idx_data)
Search for neighbors within a voxel at given point.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133