Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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>
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//////////////////////////////////////////////////////////////////////////////////////////////
52template <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//////////////////////////////////////////////////////////////////////////////////////////////
90template <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//////////////////////////////////////////////////////////////////////////////////////////////
109template <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//////////////////////////////////////////////////////////////////////////////////////////////
121template <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//////////////////////////////////////////////////////////////////////////////////////////////
139template <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//////////////////////////////////////////////////////////////////////////////////////////////
199template <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.
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.