Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id: pyramid_feature_matching.hpp 2503 2011-09-19 03:17:36Z rusu $ 00036 */ 00037 00038 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 00039 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ 00040 00041 #include <pcl/win32_macros.h> 00042 00043 #include "pcl/registration/pyramid_feature_matching.h" 00044 00049 __inline double 00050 Log2 (double n_arg) 00051 { 00052 return log (n_arg) / M_LN2; 00053 } 00054 00055 00057 template <typename PointFeature> float 00058 pcl::PyramidFeatureHistogram<PointFeature>::comparePyramidFeatureHistograms (const PyramidFeatureHistogramPtr &pyramid_a, 00059 const PyramidFeatureHistogramPtr &pyramid_b) 00060 { 00061 // do a few consistency checks before and during the computation 00062 if (pyramid_a->nr_dimensions != pyramid_b->nr_dimensions) 00063 { 00064 PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of dimensions: %u vs %u\n", pyramid_a->nr_dimensions, pyramid_b->nr_dimensions); 00065 return -1; 00066 } 00067 if (pyramid_a->nr_levels != pyramid_b->nr_levels) 00068 { 00069 PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of levels: %u vs %u\n", pyramid_a->nr_levels, pyramid_b->nr_levels); 00070 return -1; 00071 } 00072 00073 00074 // calculate for level 0 first 00075 if (pyramid_a->hist_levels[0].hist.size () != pyramid_b->hist_levels[0].hist.size ()) 00076 { 00077 PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level 0: %u vs %u\n", pyramid_a->hist_levels[0].hist.size (), pyramid_b->hist_levels[0].hist.size ()); 00078 return -1; 00079 } 00080 float match_count_level = 0.0f, match_count_prev_level = 0.0f; 00081 for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[0].hist.size (); ++bin_i) 00082 { 00083 if (pyramid_a->hist_levels[0].hist[bin_i] < pyramid_b->hist_levels[0].hist[bin_i]) 00084 match_count_level += pyramid_a->hist_levels[0].hist[bin_i]; 00085 else 00086 match_count_level += pyramid_b->hist_levels[0].hist[bin_i]; 00087 } 00088 00089 00090 float match_count = match_count_level; 00091 for (size_t level_i = 1; level_i < pyramid_a->nr_levels; ++level_i) 00092 { 00093 if (pyramid_a->hist_levels[level_i].hist.size () != pyramid_b->hist_levels[level_i].hist.size ()) 00094 { 00095 PCL_ERROR ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] The two given pyramids have different numbers of bins on level %u: %u vs %u\n", level_i, pyramid_a->hist_levels[level_i].hist.size (), pyramid_b->hist_levels[level_i].hist.size ()); 00096 return -1; 00097 } 00098 00099 match_count_prev_level = match_count_level; 00100 match_count_level = 0.0f; 00101 for (size_t bin_i = 0; bin_i < pyramid_a->hist_levels[level_i].hist.size (); ++bin_i) 00102 { 00103 if (pyramid_a->hist_levels[level_i].hist[bin_i] < pyramid_b->hist_levels[level_i].hist[bin_i]) 00104 match_count_level += pyramid_a->hist_levels[level_i].hist[bin_i]; 00105 else 00106 match_count_level += pyramid_b->hist_levels[level_i].hist[bin_i]; 00107 } 00108 00109 float level_normalization_factor = pow(2.0f, (int) level_i); 00110 match_count += (match_count_level - match_count_prev_level) / level_normalization_factor; 00111 } 00112 00113 00114 // include self-similarity factors 00115 float self_similarity_a = pyramid_a->nr_features, 00116 self_similarity_b = pyramid_b->nr_features; 00117 PCL_DEBUG ("[pcl::PyramidFeatureMatching::comparePyramidFeatureHistograms] Self similarity measures: %f, %f\n", self_similarity_a, self_similarity_b); 00118 match_count /= sqrt (self_similarity_a * self_similarity_b); 00119 00120 return match_count; 00121 } 00122 00123 00125 template <typename PointFeature> 00126 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () 00127 { 00128 feature_representation_.reset (new DefaultPointRepresentation<PointFeature>); 00129 is_computed_ = false; 00130 } 00131 00132 template <typename PointFeature> void 00133 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel () 00134 { 00135 size_t total_vector_size = 1; 00136 for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it) 00137 total_vector_size *= *dim_it; 00138 00139 hist.resize (total_vector_size, 0); 00140 } 00141 00142 00144 template <typename PointFeature> bool 00145 pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram () 00146 { 00147 // a few consistency checks before starting the computations 00148 if (!PCLBase<PointFeature>::initCompute ()) 00149 { 00150 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n"); 00151 return false; 00152 } 00153 00154 if (dimension_range_input_.size () == 0) 00155 { 00156 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n"); 00157 return false; 00158 } 00159 00160 if (dimension_range_target_.size () == 0) 00161 { 00162 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n"); 00163 return false; 00164 } 00165 00166 if (dimension_range_input_.size () != dimension_range_target_.size ()) 00167 { 00168 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n", 00169 dimension_range_input_.size (), dimension_range_target_.size ()); 00170 return false; 00171 } 00172 00173 00174 nr_dimensions = dimension_range_target_.size (); 00175 nr_features = input_->points.size (); 00176 float D = 0.0f; 00177 for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it) 00178 { 00179 float aux = range_it->first - range_it->second; 00180 D += aux * aux; 00181 } 00182 D = sqrt (D); 00183 nr_levels = ceil (Log2 (D)); 00184 PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D); 00185 00186 00187 hist_levels.resize (nr_levels); 00188 for (size_t level_i = 0; level_i < nr_levels; ++level_i) 00189 { 00190 std::vector<size_t> bins_per_dimension (nr_dimensions); 00191 std::vector<float> bin_step (nr_dimensions); 00192 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) { 00193 bins_per_dimension[dim_i] = ceil ( (dimension_range_target_[dim_i].second - dimension_range_target_[dim_i].first) / (pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions))); 00194 bin_step[dim_i] = pow (2.0f, (int) level_i) * sqrt ((float) nr_dimensions); 00195 } 00196 hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step); 00197 00198 PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i); 00199 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) 00200 PCL_DEBUG ("%u ", bins_per_dimension[dim_i]); 00201 PCL_DEBUG ("\n"); 00202 } 00203 00204 return true; 00205 } 00206 00207 00209 template <typename PointFeature> unsigned int& 00210 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access, 00211 size_t &level) 00212 { 00213 if (access.size () != nr_dimensions) 00214 { 00215 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n"); 00216 return hist_levels.front ().hist.front (); 00217 } 00218 if (level >= hist_levels.size ()) 00219 { 00220 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n"); 00221 return hist_levels.front ().hist.front (); 00222 } 00223 00224 size_t vector_position = 0; 00225 size_t dim_accumulator = 1; 00226 00227 for (int i = access.size ()-1; i >= 0; --i) 00228 { 00229 vector_position += access[i] * dim_accumulator; 00230 dim_accumulator *= hist_levels[level].bins_per_dimension[i]; 00231 } 00232 00233 return hist_levels[level].hist[vector_position]; 00234 } 00235 00236 00238 template <typename PointFeature> unsigned int& 00239 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature, 00240 size_t &level) 00241 { 00242 if (feature.size () != nr_dimensions) 00243 { 00244 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] The given feature vector does not match the feature dimensions of the pyramid histogram: %u vs %u\n", feature.size (), nr_dimensions); 00245 return hist_levels.front ().hist.front (); 00246 } 00247 if (level >= hist_levels.size ()) 00248 { 00249 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n"); 00250 return hist_levels.front ().hist.front (); 00251 } 00252 00253 std::vector<size_t> access; 00254 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i) 00255 access.push_back ( floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])); 00256 00257 return at (access, level); 00258 } 00259 00260 00262 template <typename PointFeature> void 00263 pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature, 00264 std::vector<float> &feature_vector) 00265 { 00266 // convert feature to vector representation 00267 feature_vector.resize (feature_representation_->getNumberOfDimensions ()); 00268 feature_representation_->vectorize (feature, feature_vector); 00269 00270 // adapt the values from the input range to the target range 00271 for (size_t i = 0; i < feature_vector.size (); ++i) 00272 feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) * 00273 (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first; 00274 } 00275 00276 00278 template <typename PointFeature> void 00279 pcl::PyramidFeatureHistogram<PointFeature>::compute () 00280 { 00281 if (!initializeHistogram ()) 00282 return; 00283 00284 for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i) 00285 { 00286 std::vector<float> feature_vector; 00287 convertFeatureToVector (input_->points[feature_i], feature_vector); 00288 addFeature (feature_vector); 00289 } 00290 00291 is_computed_ = true; 00292 } 00293 00294 00296 template <typename PointFeature> void 00297 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature) 00298 { 00299 for (size_t level_i = 0; level_i < nr_levels; ++level_i) 00300 at (feature, level_i) ++; 00301 } 00302 00303 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>; 00304 00305 #endif /* PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_ */