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