Point Cloud Library (PCL)  1.7.1
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.h>
46 #include <pcl/octree/octree_search.h>
47 #include <pcl/common/eigen.h>
48 
49 #include <algorithm>
50 #include <fstream>
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template<typename PointInT, typename PointNT, typename PointOutT> void
55 {
57  {
58  output.width = output.height = 0;
59  output.points.clear ();
60  return;
61  }
62  // Copy the header
63  output.header = input_->header;
64 
65  // Resize the output dataset
66  // Important! We should only allocate precisely how many elements we will need, otherwise
67  // we risk at pre-allocating too much memory which could lead to bad_alloc
68  // (see http://dev.pointclouds.org/issues/657)
69  output.width = output.height = 1;
70  output.is_dense = input_->is_dense;
71  output.points.resize (1);
72 
73  // Perform the actual feature computation
74  computeFeature (output);
75 
77 }
78 
79 //////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointInT, typename PointNT, typename PointOutT> void
82 {
83  pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
84  octree.setInputCloud (input_);
85  octree.addPointsFromInputCloud ();
86 
87  typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
88  octree.getOccupiedVoxelCenters (occupied_cells);
89 
90  // Determine the voxels crosses along the line segments
91  // formed by every pair of occupied cells.
92  std::vector< std::vector<int> > line_histograms;
93  for (size_t i = 0; i < occupied_cells.size (); ++i)
94  {
95  Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
96 
97  for (size_t j = i+1; j < occupied_cells.size (); ++j)
98  {
99  typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
100  Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
101  octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
102 
103  // Intersected cells are ordered from closest to furthest w.r.t. the origin.
104  std::vector<int> histogram;
105  for (size_t k = 0; k < intersected_cells.size (); ++k)
106  {
107  std::vector<int> indices;
108  octree.voxelSearch (intersected_cells[k], indices);
109  int label = emptyLabel ();
110  if (indices.size () != 0)
111  {
112  label = getDominantLabel (indices);
113  }
114  histogram.push_back (label);
115  }
116 
117  line_histograms.push_back(histogram);
118  }
119  }
120 
121  std::vector< std::vector<int> > transition_histograms;
122  computeTransitionHistograms (line_histograms, transition_histograms);
123 
124  std::vector<float> distances;
125  computeDistancesToMean (transition_histograms, distances);
126 
127  std::vector<float> gfpfh_histogram;
128  computeDistanceHistogram (distances, gfpfh_histogram);
129 
130  output.clear ();
131  output.width = 1;
132  output.height = 1;
133  output.points.resize (1);
134  std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
135 }
136 
137 //////////////////////////////////////////////////////////////////////////////////////////////
138 template <typename PointInT, typename PointNT, typename PointOutT> void
139 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeTransitionHistograms (const std::vector< std::vector<int> >& label_histograms,
140  std::vector< std::vector<int> >& transition_histograms)
141 {
142  transition_histograms.resize (label_histograms.size ());
143 
144  for (size_t i = 0; i < label_histograms.size (); ++i)
145  {
146  transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0);
147 
148  std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1);
149  for (size_t k = 0; k < transitions.size (); ++k)
150  {
151  transitions[k].resize (getNumberOfClasses () + 1, 0);
152  }
153 
154  for (size_t k = 1; k < label_histograms[i].size (); ++k)
155  {
156  uint32_t first_class = label_histograms[i][k-1];
157  uint32_t second_class = label_histograms[i][k];
158  // Order has no influence.
159  if (second_class < first_class)
160  std::swap (first_class, second_class);
161 
162  transitions[first_class][second_class] += 1;
163  }
164 
165  // Build a one-dimension histogram out of it.
166  int flat_index = 0;
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)
169  {
170  transition_histograms[i][flat_index] = transitions[m][n];
171  ++flat_index;
172  }
173 
174  assert (flat_index == static_cast<int> (transition_histograms[i].size ()));
175  }
176 }
177 
178 //////////////////////////////////////////////////////////////////////////////////////////////
179 template <typename PointInT, typename PointNT, typename PointOutT> void
180 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistancesToMean (const std::vector< std::vector<int> >& transition_histograms,
181  std::vector<float>& distances)
182 {
183  distances.resize (transition_histograms.size ());
184 
185  std::vector<float> mean_histogram;
186  computeMeanHistogram (transition_histograms, mean_histogram);
187 
188  for (size_t i = 0; i < transition_histograms.size (); ++i)
189  {
190  float d = computeHIKDistance (transition_histograms[i], mean_histogram);
191  distances[i] = d;
192  }
193 }
194 
195 //////////////////////////////////////////////////////////////////////////////////////////////
196 template <typename PointInT, typename PointNT, typename PointOutT> void
198  std::vector<float>& histogram)
199 {
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;
203 
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;
207 
208  histogram.resize (descriptorSize (), 0);
209 
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)
213  {
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)));
216  histogram[bin] += 1;
217  }
218 }
219 
220 //////////////////////////////////////////////////////////////////////////////////////////////
221 template <typename PointInT, typename PointNT, typename PointOutT> void
222 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeMeanHistogram (const std::vector< std::vector<int> >& histograms,
223  std::vector<float>& mean_histogram)
224 {
225  assert (histograms.size () > 0);
226 
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]);
231 
232  for (size_t i = 0; i < mean_histogram.size (); ++i)
233  mean_histogram[i] /= static_cast<float> (histograms.size ());
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////////////////////
237 template <typename PointInT, typename PointNT, typename PointOutT> float
239  const std::vector<float>& mean_histogram)
240 {
241  assert (histogram.size () == mean_histogram.size ());
242 
243  float norm = 0.f;
244  for (size_t i = 0; i < histogram.size (); ++i)
245  norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
246 
247  norm /= static_cast<float> (histogram.size ());
248  return (norm);
249 }
250 
251 //////////////////////////////////////////////////////////////////////////////////////////////
252 template <typename PointInT, typename PointNT, typename PointOutT> boost::uint32_t
254 {
255  std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0);
256  for (size_t i = 0; i < indices.size (); ++i)
257  {
258  uint32_t label = labels_->points[indices[i]].label;
259  counts[label] += 1;
260  }
261 
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 ());
266 
267  return (static_cast<uint32_t> (max_it - counts.begin ()));
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_