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>;
boost::shared_ptr< std::vector< int > > IndicesPtr
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void generateCloudGraph()
Method that generates the underlying nearest neighbor graph based on the input point cloud...
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...
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.