Point Cloud Library (PCL)  1.9.1
statistical_multiscale_interest_region_extraction.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Alexandru-Eugen Ichim
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$
38  */
39 
40 #ifndef PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
41 #define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
42 
43 #include <pcl/features/statistical_multiscale_interest_region_extraction.h>
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/common/distances.h>
46 #include <pcl/features/boost.h>
47 #include <boost/graph/adjacency_list.hpp>
48 #include <boost/graph/johnson_all_pairs_shortest.hpp>
49 
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////
52 template <typename PointT> void
54 {
55  // generate a K-NNG (K-nearest neighbors graph)
57  kdtree.setInputCloud (input_);
58 
59  using namespace boost;
60  typedef property<edge_weight_t, float> Weight;
61  typedef adjacency_list<vecS, vecS, undirectedS, no_property, Weight> Graph;
62  Graph cloud_graph;
63 
64  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
65  {
66  std::vector<int> k_indices (16);
67  std::vector<float> k_distances (16);
68  kdtree.nearestKSearch (static_cast<int> (point_i), 16, k_indices, k_distances);
69 
70  for (int k_i = 0; k_i < static_cast<int> (k_indices.size ()); ++k_i)
71  add_edge (point_i, k_indices[k_i], Weight (std::sqrt (k_distances[k_i])), cloud_graph);
72  }
73 
74  const size_t E = num_edges (cloud_graph),
75  V = num_vertices (cloud_graph);
76  PCL_INFO ("The graph has %lu vertices and %lu edges.\n", V, E);
77  geodesic_distances_.clear ();
78  for (size_t i = 0; i < V; ++i)
79  {
80  std::vector<float> aux (V);
81  geodesic_distances_.push_back (aux);
82  }
83  johnson_all_pairs_shortest_paths (cloud_graph, geodesic_distances_);
84 
85  PCL_INFO ("Done generating the graph\n");
86 }
87 
88 
89 //////////////////////////////////////////////////////////////////////////////////////////////
90 template <typename PointT> bool
92 {
94  {
95  PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
96  return (false);
97  }
98  if (scale_values_.empty ())
99  {
100  PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] No scale values were given\n");
101  return (false);
102  }
103 
104  return (true);
105 }
106 
107 
108 //////////////////////////////////////////////////////////////////////////////////////////////
109 template <typename PointT> void
111  float &radius,
112  std::vector<int> &result_indices)
113 {
114  for (size_t i = 0; i < geodesic_distances_[query_index].size (); ++i)
115  if (i != query_index && geodesic_distances_[query_index][i] < radius)
116  result_indices.push_back (static_cast<int> (i));
117 }
118 
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////
121 template <typename PointT> void
123 {
124  if (!initCompute ())
125  {
126  PCL_ERROR ("StatisticalMultiscaleInterestRegionExtraction: not completely initialized\n");
127  return;
128  }
129 
130  generateCloudGraph ();
131 
132  computeF ();
133 
134  extractExtrema (rois);
135 }
136 
137 
138 //////////////////////////////////////////////////////////////////////////////////////////////
139 template <typename PointT> void
141 {
142  PCL_INFO ("Calculating statistical information\n");
143 
144  // declare and initialize data structure
145  F_scales_.resize (scale_values_.size ());
146  std::vector<float> point_density (input_->points.size ()),
147  F (input_->points.size ());
148  std::vector<std::vector<float> > phi (input_->points.size ());
149  std::vector<float> phi_row (input_->points.size ());
150 
151  for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
152  {
153  float scale_squared = scale_values_[scale_i] * scale_values_[scale_i];
154 
155  // calculate point density for each point x_i
156  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
157  {
158  float point_density_i = 0.0;
159  for (size_t point_j = 0; point_j < input_->points.size (); ++point_j)
160  {
161  float d_g = geodesic_distances_[point_i][point_j];
162  float phi_i_j = 1.0f / std::sqrt (2.0f * static_cast<float> (M_PI) * scale_squared) * expf ( (-1) * d_g*d_g / (2.0f * scale_squared));
163 
164  point_density_i += phi_i_j;
165  phi_row[point_j] = phi_i_j;
166  }
167  point_density[point_i] = point_density_i;
168  phi[point_i] = phi_row;
169  }
170 
171  // compute weights for each pair (x_i, x_j), evaluate the operator A_hat
172  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
173  {
174  float A_hat_normalization = 0.0;
175  PointT A_hat; A_hat.x = A_hat.y = A_hat.z = 0.0;
176  for (size_t point_j = 0; point_j < input_->points.size (); ++point_j)
177  {
178  float phi_hat_i_j = phi[point_i][point_j] / (point_density[point_i] * point_density[point_j]);
179  A_hat_normalization += phi_hat_i_j;
180 
181  PointT aux = input_->points[point_j];
182  aux.x *= phi_hat_i_j; aux.y *= phi_hat_i_j; aux.z *= phi_hat_i_j;
183 
184  A_hat.x += aux.x; A_hat.y += aux.y; A_hat.z += aux.z;
185  }
186  A_hat.x /= A_hat_normalization; A_hat.y /= A_hat_normalization; A_hat.z /= A_hat_normalization;
187 
188  // compute the invariant F
189  float aux = 2.0f / scale_values_[scale_i] * euclideanDistance<PointT, PointT> (A_hat, input_->points[point_i]);
190  F[point_i] = aux * expf (-aux);
191  }
192 
193  F_scales_[scale_i] = F;
194  }
195 }
196 
197 
198 //////////////////////////////////////////////////////////////////////////////////////////////
199 template <typename PointT> void
201 {
202  std::vector<std::vector<bool> > is_min (scale_values_.size ()),
203  is_max (scale_values_.size ());
204 
205  // for each point, check if it is a local extrema on each scale
206  for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
207  {
208  std::vector<bool> is_min_scale (input_->points.size ()),
209  is_max_scale (input_->points.size ());
210  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
211  {
212  std::vector<int> nn_indices;
213  geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
214  bool is_max_point = true, is_min_point = true;
215  for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
216  if (F_scales_[scale_i][point_i] < F_scales_[scale_i][*nn_it])
217  is_max_point = false;
218  else
219  is_min_point = false;
220 
221  is_min_scale[point_i] = is_min_point;
222  is_max_scale[point_i] = is_max_point;
223  }
224 
225  is_min[scale_i] = is_min_scale;
226  is_max[scale_i] = is_max_scale;
227  }
228 
229  // look for points that are min/max over three consecutive scales
230  for (size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i)
231  {
232  for (size_t point_i = 0; point_i < input_->points.size (); ++point_i)
233  if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] && is_min[scale_i + 1][point_i]) ||
234  (is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i]))
235  {
236  // add the point to the result vector
237  IndicesPtr region (new std::vector<int>);
238  region->push_back (static_cast<int> (point_i));
239 
240  // and also add its scale-sized geodesic neighborhood
241  std::vector<int> nn_indices;
242  geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices);
243  region->insert (region->end (), nn_indices.begin (), nn_indices.end ());
244  rois.push_back (region);
245  }
246  }
247 }
248 
249 
250 #define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T) template class PCL_EXPORTS pcl::StatisticalMultiscaleInterestRegionExtraction<T>;
251 
252 #endif /* PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ */
253 
pcl::IndicesPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
pcl::StatisticalMultiscaleInterestRegionExtraction
Class for extracting interest regions from unstructured point clouds, based on a multi scale statisti...
Definition: statistical_multiscale_interest_region_extraction.h:65
pcl::StatisticalMultiscaleInterestRegionExtraction::generateCloudGraph
void generateCloudGraph()
Method that generates the underlying nearest neighbor graph based on the input point cloud.
Definition: statistical_multiscale_interest_region_extraction.hpp:53
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:68
boost
Definition: boost_graph.h:47
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:619
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:92
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:69
pcl::KdTreeFLANN::nearestKSearch
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Definition: kdtree_flann.hpp:132
pcl::StatisticalMultiscaleInterestRegionExtraction::computeRegionsOfInterest
void computeRegionsOfInterest(std::list< IndicesPtr > &rois)
The method to be called in order to run the algorithm and produce the resulting set of regions of int...
Definition: statistical_multiscale_interest_region_extraction.hpp:122
distances.h