Point Cloud Library (PCL)  1.9.0-dev
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 <pcl/common/eigen.h>
47 
48 #include <algorithm>
49 #include <fstream>
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////
52 template<typename PointInT, typename PointNT, typename PointOutT> void
54 {
56  {
57  output.width = output.height = 0;
58  output.points.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.points.resize (1);
71 
72  // Perform the actual feature computation
73  computeFeature (output);
74 
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointInT, typename PointNT, typename PointOutT> void
81 {
82  pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
83  octree.setInputCloud (input_);
84  octree.addPointsFromInputCloud ();
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 (size_t i = 0; i < occupied_cells.size (); ++i)
93  {
94  Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
95 
96  for (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 (size_t k = 0; k < intersected_cells.size (); ++k)
105  {
106  std::vector<int> indices;
107  octree.voxelSearch (intersected_cells[k], indices);
108  int label = emptyLabel ();
109  if (indices.size () != 0)
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.points.resize (1);
133  std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
134 }
135 
136 //////////////////////////////////////////////////////////////////////////////////////////////
137 template <typename PointInT, typename PointNT, typename PointOutT> void
138 pcl::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 (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 (size_t k = 0; k < transitions.size (); ++k)
149  {
150  transitions[k].resize (getNumberOfClasses () + 1, 0);
151  }
152 
153  for (size_t k = 1; k < label_histograms[i].size (); ++k)
154  {
155  uint32_t first_class = label_histograms[i][k-1];
156  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  int flat_index = 0;
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)
168  {
169  transition_histograms[i][flat_index] = transitions[m][n];
170  ++flat_index;
171  }
172 
173  assert (flat_index == static_cast<int> (transition_histograms[i].size ()));
174  }
175 }
176 
177 //////////////////////////////////////////////////////////////////////////////////////////////
178 template <typename PointInT, typename PointNT, typename PointOutT> void
179 pcl::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 (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 //////////////////////////////////////////////////////////////////////////////////////////////
195 template <typename PointInT, typename PointNT, typename PointOutT> void
197  std::vector<float>& histogram)
198 {
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;
202 
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;
206 
207  histogram.resize (descriptorSize (), 0);
208 
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)
212  {
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)));
215  histogram[bin] += 1;
216  }
217 }
218 
219 //////////////////////////////////////////////////////////////////////////////////////////////
220 template <typename PointInT, typename PointNT, typename PointOutT> void
221 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeMeanHistogram (const std::vector< std::vector<int> >& histograms,
222  std::vector<float>& mean_histogram)
223 {
224  assert (histograms.size () > 0);
225 
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]);
230 
231  for (size_t i = 0; i < mean_histogram.size (); ++i)
232  mean_histogram[i] /= static_cast<float> (histograms.size ());
233 }
234 
235 //////////////////////////////////////////////////////////////////////////////////////////////
236 template <typename PointInT, typename PointNT, typename PointOutT> float
238  const std::vector<float>& mean_histogram)
239 {
240  assert (histogram.size () == mean_histogram.size ());
241 
242  float norm = 0.f;
243  for (size_t i = 0; i < histogram.size (); ++i)
244  norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
245 
246  norm /= static_cast<float> (histogram.size ());
247  return (norm);
248 }
249 
250 //////////////////////////////////////////////////////////////////////////////////////////////
251 template <typename PointInT, typename PointNT, typename PointOutT> boost::uint32_t
253 {
254  std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0);
255  for (size_t i = 0; i < indices.size (); ++i)
256  {
257  uint32_t label = labels_->points[indices[i]].label;
258  counts[label] += 1;
259  }
260 
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 ());
265 
266  return (static_cast<uint32_t> (max_it - counts.begin ()));
267 }
268 
269 #define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>;
270 
271 #endif // PCL_FEATURES_IMPL_GFPFH_H_
void computeFeature(PointCloudOut &output)
Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by <setInputCloud ()...
Definition: gfpfh.hpp:80
size_t size() const
Definition: point_cloud.h:448
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.
Definition: point_cloud.h:410
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
void computeDistanceHistogram(const std::vector< float > &distances, std::vector< float > &histogram)
Compute the binned histogram of distance values.
Definition: gfpfh.hpp:196
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
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: gfpfh.hpp:53
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).
Definition: point_cloud.h:415
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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.
Definition: gfpfh.hpp:221
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
float computeHIKDistance(const std::vector< int > &histogram, const std::vector< float > &mean_histogram)
Return the Intersection Kernel distance between two histograms.
Definition: gfpfh.hpp:237
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
Octree pointcloud search class
Definition: octree_search.h:57
uint32_t getDominantLabel(const std::vector< int > &indices)
Return the dominant label of a set of points.
Definition: gfpfh.hpp:252
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:418
Feature represents the base feature class.
Definition: feature.h:105