Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
00039 #define PCL_REGISTRATION_IMPL_PYRAMID_FEATURE_MATCHING_H_
00040
00041 #include <pcl/pcl_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
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
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 += static_cast<float> (pyramid_a->hist_levels[0].hist[bin_i]);
00085 else
00086 match_count_level += static_cast<float> (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 += static_cast<float> (pyramid_a->hist_levels[level_i].hist[bin_i]);
00105 else
00106 match_count_level += static_cast<float> (pyramid_b->hist_levels[level_i].hist[bin_i]);
00107 }
00108
00109 float level_normalization_factor = powf (2.0f, static_cast<float> (level_i));
00110 match_count += (match_count_level - match_count_prev_level) / level_normalization_factor;
00111 }
00112
00113
00114
00115 float self_similarity_a = static_cast<float> (pyramid_a->nr_features),
00116 self_similarity_b = static_cast<float> (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 /= sqrtf (self_similarity_a * self_similarity_b);
00119
00120 return match_count;
00121 }
00122
00123
00125 template <typename PointFeature>
00126 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogram () :
00127 nr_dimensions (0), nr_levels (0), nr_features (0),
00128 dimension_range_input_ (), dimension_range_target_ (),
00129 feature_representation_ (new DefaultPointRepresentation<PointFeature>),
00130 is_computed_ (false),
00131 hist_levels ()
00132 {
00133 }
00134
00136 template <typename PointFeature> void
00137 pcl::PyramidFeatureHistogram<PointFeature>::PyramidFeatureHistogramLevel::initializeHistogramLevel ()
00138 {
00139 size_t total_vector_size = 1;
00140 for (std::vector<size_t>::iterator dim_it = bins_per_dimension.begin (); dim_it != bins_per_dimension.end (); ++dim_it)
00141 total_vector_size *= *dim_it;
00142
00143 hist.resize (total_vector_size, 0);
00144 }
00145
00146
00148 template <typename PointFeature> bool
00149 pcl::PyramidFeatureHistogram<PointFeature>::initializeHistogram ()
00150 {
00151
00152 if (!PCLBase<PointFeature>::initCompute ())
00153 {
00154 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] PCLBase initCompute failed\n");
00155 return false;
00156 }
00157
00158 if (dimension_range_input_.size () == 0)
00159 {
00160 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input dimension range was not set\n");
00161 return false;
00162 }
00163
00164 if (dimension_range_target_.size () == 0)
00165 {
00166 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Target dimension range was not set\n");
00167 return false;
00168 }
00169
00170 if (dimension_range_input_.size () != dimension_range_target_.size ())
00171 {
00172 PCL_ERROR ("[pcl::PyramidFeatureHistogram::initializeHistogram] Input and target dimension ranges do not agree in size: %u vs %u\n",
00173 dimension_range_input_.size (), dimension_range_target_.size ());
00174 return false;
00175 }
00176
00177
00178 nr_dimensions = dimension_range_target_.size ();
00179 nr_features = input_->points.size ();
00180 float D = 0.0f;
00181 for (std::vector<std::pair<float, float> >::iterator range_it = dimension_range_target_.begin (); range_it != dimension_range_target_.end (); ++range_it)
00182 {
00183 float aux = range_it->first - range_it->second;
00184 D += aux * aux;
00185 }
00186 D = sqrtf (D);
00187 nr_levels = static_cast<size_t> (ceilf (log2f (D)));
00188 PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Pyramid will have %u levels with a hyper-parallelepiped diagonal size of %f\n", nr_levels, D);
00189
00190
00191 hist_levels.resize (nr_levels);
00192 for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00193 {
00194 std::vector<size_t> bins_per_dimension (nr_dimensions);
00195 std::vector<float> bin_step (nr_dimensions);
00196 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00197 {
00198 bins_per_dimension[dim_i] =
00199 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)))));
00200 bin_step[dim_i] = powf (2.0f, static_cast<float> (level_i)) * sqrtf (static_cast<float> (nr_dimensions));
00201 }
00202 hist_levels[level_i] = PyramidFeatureHistogramLevel (bins_per_dimension, bin_step);
00203
00204 PCL_DEBUG ("[pcl::PyramidFeatureHistogram::initializeHistogram] Created vector of size %u at level %u\nwith #bins per dimension:", hist_levels.back ().hist.size (), level_i);
00205 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00206 PCL_DEBUG ("%u ", bins_per_dimension[dim_i]);
00207 PCL_DEBUG ("\n");
00208 }
00209
00210 return true;
00211 }
00212
00213
00215 template <typename PointFeature> unsigned int&
00216 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<size_t> &access,
00217 size_t &level)
00218 {
00219 if (access.size () != nr_dimensions)
00220 {
00221 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Cannot access histogram position because the access point does not have the right number of dimensions\n");
00222 return hist_levels.front ().hist.front ();
00223 }
00224 if (level >= hist_levels.size ())
00225 {
00226 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00227 return hist_levels.front ().hist.front ();
00228 }
00229
00230 size_t vector_position = 0;
00231 size_t dim_accumulator = 1;
00232
00233 for (int i = static_cast<int> (access.size ()) - 1; i >= 0; --i)
00234 {
00235 vector_position += access[i] * dim_accumulator;
00236 dim_accumulator *= hist_levels[level].bins_per_dimension[i];
00237 }
00238
00239 return hist_levels[level].hist[vector_position];
00240 }
00241
00242
00244 template <typename PointFeature> unsigned int&
00245 pcl::PyramidFeatureHistogram<PointFeature>::at (std::vector<float> &feature,
00246 size_t &level)
00247 {
00248 if (feature.size () != nr_dimensions)
00249 {
00250 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);
00251 return hist_levels.front ().hist.front ();
00252 }
00253 if (level >= hist_levels.size ())
00254 {
00255 PCL_ERROR ("[pcl::PyramidFeatureHistogram::at] Trying to access a too large level\n");
00256 return hist_levels.front ().hist.front ();
00257 }
00258
00259 std::vector<size_t> access;
00260 for (size_t dim_i = 0; dim_i < nr_dimensions; ++dim_i)
00261 access.push_back (static_cast<size_t> (floor ((feature[dim_i] - dimension_range_target_[dim_i].first) / hist_levels[level].bin_step[dim_i])));
00262
00263 return at (access, level);
00264 }
00265
00266
00268 template <typename PointFeature> void
00269 pcl::PyramidFeatureHistogram<PointFeature>::convertFeatureToVector (const PointFeature &feature,
00270 std::vector<float> &feature_vector)
00271 {
00272
00273 feature_vector.resize (feature_representation_->getNumberOfDimensions ());
00274 feature_representation_->vectorize (feature, feature_vector);
00275
00276
00277 for (size_t i = 0; i < feature_vector.size (); ++i)
00278 feature_vector[i] = (feature_vector[i] - dimension_range_input_[i].first) / (dimension_range_input_[i].second - dimension_range_input_[i].first) *
00279 (dimension_range_target_[i].second - dimension_range_target_[i].first) + dimension_range_target_[i].first;
00280 }
00281
00282
00284 template <typename PointFeature> void
00285 pcl::PyramidFeatureHistogram<PointFeature>::compute ()
00286 {
00287 if (!initializeHistogram ())
00288 return;
00289
00290 for (size_t feature_i = 0; feature_i < input_->points.size (); ++feature_i)
00291 {
00292 std::vector<float> feature_vector;
00293 convertFeatureToVector (input_->points[feature_i], feature_vector);
00294 addFeature (feature_vector);
00295 }
00296
00297 is_computed_ = true;
00298 }
00299
00300
00302 template <typename PointFeature> void
00303 pcl::PyramidFeatureHistogram<PointFeature>::addFeature (std::vector<float> &feature)
00304 {
00305 for (size_t level_i = 0; level_i < nr_levels; ++level_i)
00306 at (feature, level_i) ++;
00307 }
00308
00309 #define PCL_INSTANTIATE_PyramidFeatureHistogram(PointFeature) template class PCL_EXPORTS pcl::PyramidFeatureHistogram<PointFeature>;
00310
00311 #endif