40 #ifndef PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
41 #define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_
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>
52 template <
typename Po
intT>
void
59 using namespace boost;
60 typedef property<edge_weight_t, float> Weight;
61 typedef adjacency_list<vecS, vecS, undirectedS, no_property, Weight> Graph;
64 for (
size_t point_i = 0; point_i < input_->points.size (); ++point_i)
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);
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 (sqrtf (k_distances[k_i])), cloud_graph);
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)
80 std::vector<float> aux (V);
81 geodesic_distances_.push_back (aux);
83 johnson_all_pairs_shortest_paths (cloud_graph, geodesic_distances_);
85 PCL_INFO (
"Done generating the graph\n");
90 template <
typename Po
intT>
bool
95 PCL_ERROR (
"[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n");
98 if (scale_values_.empty ())
100 PCL_ERROR (
"[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] No scale values were given\n");
109 template <
typename Po
intT>
void
112 std::vector<int> &result_indices)
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));
121 template <
typename Po
intT>
void
126 PCL_ERROR (
"StatisticalMultiscaleInterestRegionExtraction: not completely initialized\n");
130 generateCloudGraph ();
134 extractExtrema (rois);
139 template <
typename Po
intT>
void
142 PCL_INFO (
"Calculating statistical information\n");
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 ());
151 for (
size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
153 float scale_squared = scale_values_[scale_i] * scale_values_[scale_i];
156 for (
size_t point_i = 0; point_i < input_->points.size (); ++point_i)
158 float point_density_i = 0.0;
159 for (
size_t point_j = 0; point_j < input_->points.size (); ++point_j)
161 float d_g = geodesic_distances_[point_i][point_j];
162 float phi_i_j = 1.0f / sqrtf (2.0f * static_cast<float> (M_PI) * scale_squared) * expf ( (-1) * d_g*d_g / (2.0f * scale_squared));
164 point_density_i += phi_i_j;
165 phi_row[point_j] = phi_i_j;
167 point_density[point_i] = point_density_i;
168 phi[point_i] = phi_row;
172 for (
size_t point_i = 0; point_i < input_->points.size (); ++point_i)
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)
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;
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;
184 A_hat.x += aux.x; A_hat.y += aux.y; A_hat.z += aux.z;
186 A_hat.x /= A_hat_normalization; A_hat.y /= A_hat_normalization; A_hat.z /= A_hat_normalization;
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);
193 F_scales_[scale_i] = F;
199 template <
typename Po
intT>
void
202 std::vector<std::vector<bool> > is_min (scale_values_.size ()),
203 is_max (scale_values_.size ());
206 for (
size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i)
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)
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;
219 is_min_point =
false;
221 is_min_scale[point_i] = is_min_point;
222 is_max_scale[point_i] = is_max_point;
225 is_min[scale_i] = is_min_scale;
226 is_max[scale_i] = is_max_scale;
230 for (
size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i)
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]))
238 region->push_back (static_cast<int> (point_i));
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);
250 #define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T) template class PCL_EXPORTS pcl::StatisticalMultiscaleInterestRegionExtraction<T>;
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
boost::shared_ptr< std::vector< int > > IndicesPtr
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.