Point Cloud Library (PCL)  1.14.1-dev
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/console/print.h> // for PCL_INFO, PCL_ERROR
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  using Weight = property<edge_weight_t, float>;
61  using Graph = adjacency_list<vecS, vecS, undirectedS, no_property, Weight>;
62  Graph cloud_graph;
63 
64  for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
65  {
66  pcl::Indices 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 (std::size_t k_i = 0; k_i < 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 std::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 (std::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 (std::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_->size ()),
147  F (input_->size ());
148  std::vector<std::vector<float> > phi (input_->size ());
149  std::vector<float> phi_row (input_->size ());
150 
151  for (std::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 (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
157  {
158  float point_density_i = 0.0;
159  for (std::size_t point_j = 0; point_j < input_->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) * std::exp ( (-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 (std::size_t point_i = 0; point_i < input_->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 (std::size_t point_j = 0; point_j < input_->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_)[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_)[point_i]);
190  F[point_i] = aux * std::exp (-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 (std::size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
207  {
208  std::vector<bool> is_min_scale (input_->size ()),
209  is_max_scale (input_->size ());
210  for (std::size_t point_i = 0; point_i < input_->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 (const auto &nn_index : nn_indices)
216  if (F_scales_[scale_i][point_i] < F_scales_[scale_i][nn_index])
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 (std::size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i)
231  {
232  for (std::size_t point_i = 0; point_i < input_->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 pcl::Indices);
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 
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
PCL base class.
Definition: pcl_base.h:70
Class for extracting interest regions from unstructured point clouds, based on a multi scale statisti...
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...
void generateCloudGraph()
Method that generates the underlying nearest neighbor graph based on the input point cloud.
Define standard C methods to do distance calculations.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
#define M_PI
Definition: pcl_macros.h:201
A point structure representing Euclidean xyz coordinates, and the RGB color.