Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2011, Alexandru-Eugen Ichim 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ 00041 #define PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ 00042 00043 #include <pcl/features/statistical_multiscale_interest_region_extraction.h> 00044 #include <pcl/kdtree/kdtree_flann.h> 00045 #include <pcl/common/distances.h> 00046 #include <pcl/features/boost.h> 00047 #include <boost/graph/adjacency_list.hpp> 00048 #include <boost/graph/johnson_all_pairs_shortest.hpp> 00049 00050 00051 ////////////////////////////////////////////////////////////////////////////////////////////// 00052 template <typename PointT> void 00053 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::generateCloudGraph () 00054 { 00055 // generate a K-NNG (K-nearest neighbors graph) 00056 pcl::KdTreeFLANN<PointT> kdtree; 00057 kdtree.setInputCloud (input_); 00058 00059 using namespace boost; 00060 typedef property<edge_weight_t, float> Weight; 00061 typedef adjacency_list<vecS, vecS, undirectedS, no_property, Weight> Graph; 00062 Graph cloud_graph; 00063 00064 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00065 { 00066 std::vector<int> k_indices (16); 00067 std::vector<float> k_distances (16); 00068 kdtree.nearestKSearch (static_cast<int> (point_i), 16, k_indices, k_distances); 00069 00070 for (int k_i = 0; k_i < static_cast<int> (k_indices.size ()); ++k_i) 00071 add_edge (point_i, k_indices[k_i], Weight (sqrtf (k_distances[k_i])), cloud_graph); 00072 } 00073 00074 const size_t E = num_edges (cloud_graph), 00075 V = num_vertices (cloud_graph); 00076 PCL_INFO ("The graph has %lu vertices and %lu edges.\n", V, E); 00077 geodesic_distances_.clear (); 00078 for (size_t i = 0; i < V; ++i) 00079 { 00080 std::vector<float> aux (V); 00081 geodesic_distances_.push_back (aux); 00082 } 00083 johnson_all_pairs_shortest_paths (cloud_graph, geodesic_distances_); 00084 00085 PCL_INFO ("Done generating the graph\n"); 00086 } 00087 00088 00089 ////////////////////////////////////////////////////////////////////////////////////////////// 00090 template <typename PointT> bool 00091 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::initCompute () 00092 { 00093 if (!PCLBase<PointT>::initCompute ()) 00094 { 00095 PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] PCLBase::initCompute () failed - no input cloud was given.\n"); 00096 return (false); 00097 } 00098 if (scale_values_.empty ()) 00099 { 00100 PCL_ERROR ("[pcl::StatisticalMultiscaleInterestRegionExtraction::initCompute] No scale values were given\n"); 00101 return (false); 00102 } 00103 00104 return (true); 00105 } 00106 00107 00108 ////////////////////////////////////////////////////////////////////////////////////////////// 00109 template <typename PointT> void 00110 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::geodesicFixedRadiusSearch (size_t &query_index, 00111 float &radius, 00112 std::vector<int> &result_indices) 00113 { 00114 for (size_t i = 0; i < geodesic_distances_[query_index].size (); ++i) 00115 if (i != query_index && geodesic_distances_[query_index][i] < radius) 00116 result_indices.push_back (static_cast<int> (i)); 00117 } 00118 00119 00120 ////////////////////////////////////////////////////////////////////////////////////////////// 00121 template <typename PointT> void 00122 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::computeRegionsOfInterest (std::list<IndicesPtr> &rois) 00123 { 00124 if (!initCompute ()) 00125 { 00126 PCL_ERROR ("StatisticalMultiscaleInterestRegionExtraction: not completely initialized\n"); 00127 return; 00128 } 00129 00130 generateCloudGraph (); 00131 00132 computeF (); 00133 00134 extractExtrema (rois); 00135 } 00136 00137 00138 ////////////////////////////////////////////////////////////////////////////////////////////// 00139 template <typename PointT> void 00140 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::computeF () 00141 { 00142 PCL_INFO ("Calculating statistical information\n"); 00143 00144 // declare and initialize data structure 00145 F_scales_.resize (scale_values_.size ()); 00146 std::vector<float> point_density (input_->points.size ()), 00147 F (input_->points.size ()); 00148 std::vector<std::vector<float> > phi (input_->points.size ()); 00149 std::vector<float> phi_row (input_->points.size ()); 00150 00151 for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) 00152 { 00153 float scale_squared = scale_values_[scale_i] * scale_values_[scale_i]; 00154 00155 // calculate point density for each point x_i 00156 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00157 { 00158 float point_density_i = 0.0; 00159 for (size_t point_j = 0; point_j < input_->points.size (); ++point_j) 00160 { 00161 float d_g = geodesic_distances_[point_i][point_j]; 00162 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)); 00163 00164 point_density_i += phi_i_j; 00165 phi_row[point_j] = phi_i_j; 00166 } 00167 point_density[point_i] = point_density_i; 00168 phi[point_i] = phi_row; 00169 } 00170 00171 // compute weights for each pair (x_i, x_j), evaluate the operator A_hat 00172 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00173 { 00174 float A_hat_normalization = 0.0; 00175 PointT A_hat; A_hat.x = A_hat.y = A_hat.z = 0.0; 00176 for (size_t point_j = 0; point_j < input_->points.size (); ++point_j) 00177 { 00178 float phi_hat_i_j = phi[point_i][point_j] / (point_density[point_i] * point_density[point_j]); 00179 A_hat_normalization += phi_hat_i_j; 00180 00181 PointT aux = input_->points[point_j]; 00182 aux.x *= phi_hat_i_j; aux.y *= phi_hat_i_j; aux.z *= phi_hat_i_j; 00183 00184 A_hat.x += aux.x; A_hat.y += aux.y; A_hat.z += aux.z; 00185 } 00186 A_hat.x /= A_hat_normalization; A_hat.y /= A_hat_normalization; A_hat.z /= A_hat_normalization; 00187 00188 // compute the invariant F 00189 float aux = 2.0f / scale_values_[scale_i] * euclideanDistance<PointT, PointT> (A_hat, input_->points[point_i]); 00190 F[point_i] = aux * expf (-aux); 00191 } 00192 00193 F_scales_[scale_i] = F; 00194 } 00195 } 00196 00197 00198 ////////////////////////////////////////////////////////////////////////////////////////////// 00199 template <typename PointT> void 00200 pcl::StatisticalMultiscaleInterestRegionExtraction<PointT>::extractExtrema (std::list<IndicesPtr> &rois) 00201 { 00202 std::vector<std::vector<bool> > is_min (scale_values_.size ()), 00203 is_max (scale_values_.size ()); 00204 00205 // for each point, check if it is a local extrema on each scale 00206 for (size_t scale_i = 0; scale_i < scale_values_.size (); ++scale_i) 00207 { 00208 std::vector<bool> is_min_scale (input_->points.size ()), 00209 is_max_scale (input_->points.size ()); 00210 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00211 { 00212 std::vector<int> nn_indices; 00213 geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices); 00214 bool is_max_point = true, is_min_point = true; 00215 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it) 00216 if (F_scales_[scale_i][point_i] < F_scales_[scale_i][*nn_it]) 00217 is_max_point = false; 00218 else 00219 is_min_point = false; 00220 00221 is_min_scale[point_i] = is_min_point; 00222 is_max_scale[point_i] = is_max_point; 00223 } 00224 00225 is_min[scale_i] = is_min_scale; 00226 is_max[scale_i] = is_max_scale; 00227 } 00228 00229 // look for points that are min/max over three consecutive scales 00230 for (size_t scale_i = 1; scale_i < scale_values_.size () - 1; ++scale_i) 00231 { 00232 for (size_t point_i = 0; point_i < input_->points.size (); ++point_i) 00233 if ((is_min[scale_i - 1][point_i] && is_min[scale_i][point_i] && is_min[scale_i + 1][point_i]) || 00234 (is_max[scale_i - 1][point_i] && is_max[scale_i][point_i] && is_max[scale_i + 1][point_i])) 00235 { 00236 // add the point to the result vector 00237 IndicesPtr region (new std::vector<int>); 00238 region->push_back (static_cast<int> (point_i)); 00239 00240 // and also add its scale-sized geodesic neighborhood 00241 std::vector<int> nn_indices; 00242 geodesicFixedRadiusSearch (point_i, scale_values_[scale_i], nn_indices); 00243 region->insert (region->end (), nn_indices.begin (), nn_indices.end ()); 00244 rois.push_back (region); 00245 } 00246 } 00247 } 00248 00249 00250 #define PCL_INSTANTIATE_StatisticalMultiscaleInterestRegionExtraction(T) template class PCL_EXPORTS pcl::StatisticalMultiscaleInterestRegionExtraction<T>; 00251 00252 #endif /* PCL_FEATURES_IMPL_STATISTICAL_MULTISCALE_INTEREST_REGION_EXTRACTION_H_ */ 00253