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.h>
46 #include <pcl/octree/octree_search.h>
47 #include <pcl/common/eigen.h>
53 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
63 output.
header = input_->header;
74 computeFeature (output);
80 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
92 std::vector< std::vector<int> > line_histograms;
93 for (
size_t i = 0; i < occupied_cells.
size (); ++i)
95 Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
97 for (
size_t j = i+1; j < occupied_cells.
size (); ++j)
100 Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
104 std::vector<int> histogram;
105 for (
size_t k = 0; k < intersected_cells.
size (); ++k)
107 std::vector<int> indices;
108 octree.
voxelSearch (intersected_cells[k], indices);
109 int label = emptyLabel ();
110 if (indices.size () != 0)
112 label = getDominantLabel (indices);
114 histogram.push_back (label);
117 line_histograms.push_back(histogram);
121 std::vector< std::vector<int> > transition_histograms;
122 computeTransitionHistograms (line_histograms, transition_histograms);
124 std::vector<float> distances;
125 computeDistancesToMean (transition_histograms, distances);
127 std::vector<float> gfpfh_histogram;
128 computeDistanceHistogram (distances, gfpfh_histogram);
134 std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.
points[0].histogram);
138 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
140 std::vector< std::vector<int> >& transition_histograms)
142 transition_histograms.resize (label_histograms.size ());
144 for (
size_t i = 0; i < label_histograms.size (); ++i)
146 transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0);
148 std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1);
149 for (
size_t k = 0; k < transitions.size (); ++k)
151 transitions[k].resize (getNumberOfClasses () + 1, 0);
154 for (
size_t k = 1; k < label_histograms[i].size (); ++k)
156 uint32_t first_class = label_histograms[i][k-1];
157 uint32_t second_class = label_histograms[i][k];
159 if (second_class < first_class)
160 std::swap (first_class, second_class);
162 transitions[first_class][second_class] += 1;
167 for (
int m = 0; m < static_cast<int> (transitions.size ()); ++m)
168 for (
int n = m; n < static_cast<int> (transitions[m].size ()); ++n)
170 transition_histograms[i][flat_index] = transitions[m][n];
174 assert (flat_index == static_cast<int> (transition_histograms[i].size ()));
179 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
181 std::vector<float>& distances)
183 distances.resize (transition_histograms.size ());
185 std::vector<float> mean_histogram;
186 computeMeanHistogram (transition_histograms, mean_histogram);
188 for (
size_t i = 0; i < transition_histograms.size (); ++i)
190 float d = computeHIKDistance (transition_histograms[i], mean_histogram);
196 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
198 std::vector<float>& histogram)
200 std::vector<float>::const_iterator min_it = std::min_element (distances.begin (), distances.end ());
201 assert (min_it != distances.end ());
202 const float min_value = *min_it;
204 std::vector<float>::const_iterator max_it = std::max_element (distances.begin (), distances.end ());
205 assert (max_it != distances.end());
206 const float max_value = *max_it;
208 histogram.resize (descriptorSize (), 0);
210 const float range = max_value - min_value;
211 const int max_bin = descriptorSize () - 1;
212 for (
size_t i = 0; i < distances.size (); ++i)
214 const float raw_bin =
static_cast<float> (descriptorSize ()) * (distances[i] - min_value) / range;
215 int bin = std::min (max_bin, static_cast<int> (floor (raw_bin)));
221 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
223 std::vector<float>& mean_histogram)
225 assert (histograms.size () > 0);
227 mean_histogram.resize (histograms[0].size (), 0);
228 for (
size_t i = 0; i < histograms.size (); ++i)
229 for (
size_t j = 0; j < histograms[i].size (); ++j)
230 mean_histogram[j] += static_cast<float> (histograms[i][j]);
232 for (
size_t i = 0; i < mean_histogram.size (); ++i)
233 mean_histogram[i] /= static_cast<float> (histograms.size ());
237 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
float
239 const std::vector<float>& mean_histogram)
241 assert (histogram.size () == mean_histogram.size ());
244 for (
size_t i = 0; i < histogram.size (); ++i)
245 norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
247 norm /=
static_cast<float> (histogram.size ());
252 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> boost::uint32_t
255 std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0);
256 for (
size_t i = 0; i < indices.size (); ++i)
258 uint32_t label = labels_->points[indices[i]].label;
262 std::vector<uint32_t>::const_iterator max_it;
263 max_it = std::max_element (counts.begin (), counts.end ());
264 if (max_it == counts.end ())
265 return (emptyLabel ());
267 return (static_cast<uint32_t> (max_it - counts.begin ()));
270 #define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>;
272 #endif // PCL_FEATURES_IMPL_GFPFH_H_