From 8942f51e570a0f5a0a2ffde0883c798737ff3b4e Mon Sep 17 00:00:00 2001 From: Giacomo Dabisias Date: Mon, 8 Sep 2014 12:09:55 +0200 Subject: [PATCH] Moved pcl::PPFHashMapSearch::setInputFeatureCloud() and pcl::PPFHashMapSearch::nearestNeighborSearch() implementation from ppf_registration.hpp to ppf_registration.cpp since they belong to a not templated class --- .../registration/impl/ppf_registration.hpp | 57 --------------- .../pcl/registration/ppf_registration.h | 4 +- registration/src/ppf_registration.cpp | 71 ++++++++++++++++--- 3 files changed, 65 insertions(+), 67 deletions(-) diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp index fc752a48e1f..f9cdbd2c7e7 100644 --- a/registration/include/pcl/registration/impl/ppf_registration.hpp +++ b/registration/include/pcl/registration/impl/ppf_registration.hpp @@ -48,63 +48,6 @@ #include #include -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud::ConstPtr feature_cloud) -{ - // Discretize the feature cloud and insert it in the hash map - feature_hash_map_->clear (); - unsigned int n = static_cast (sqrt (static_cast (feature_cloud->points.size ()))); - int d1, d2, d3, d4; - max_dist_ = -1.0; - alpha_m_.resize (n); - for (size_t i = 0; i < n; ++i) - { - std::vector alpha_m_row (n); - for (size_t j = 0; j < n; ++j) - { - d1 = static_cast (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_)); - d2 = static_cast (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_)); - d3 = static_cast (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_)); - d4 = static_cast (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_)); - feature_hash_map_->insert (std::pair > (HashKeyStruct (d1, d2, d3, d4), std::pair (i, j))); - alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m; - - if (max_dist_ < feature_cloud->points[i*n + j].f4) - max_dist_ = feature_cloud->points[i*n + j].f4; - } - alpha_m_[i] = alpha_m_row; - } - - internals_initialized_ = true; -} - - -////////////////////////////////////////////////////////////////////////////////////////////// -void -pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4, - std::vector > &indices) -{ - if (!internals_initialized_) - { - PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n"); - return; - } - - int d1 = static_cast (floor (f1 / angle_discretization_step_)), - d2 = static_cast (floor (f2 / angle_discretization_step_)), - d3 = static_cast (floor (f3 / angle_discretization_step_)), - d4 = static_cast (floor (f4 / distance_discretization_step_)); - - indices.clear (); - HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4); - std::pair map_iterator_pair = feature_hash_map_->equal_range (key); - for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first) - indices.push_back (std::pair (map_iterator_pair.first->second.first, - map_iterator_pair.first->second.second)); -} - - ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PPFRegistration::setInputTarget (const PointCloudTargetConstPtr &cloud) diff --git a/registration/include/pcl/registration/ppf_registration.h b/registration/include/pcl/registration/ppf_registration.h index 2560ec6fd63..0e67c70c407 100644 --- a/registration/include/pcl/registration/ppf_registration.h +++ b/registration/include/pcl/registration/ppf_registration.h @@ -285,7 +285,7 @@ namespace pcl Eigen::Affine3f &pose2); }; } - +#ifdef PCL_NO_PRECOMPILE #include - +#endif // PCL_NO_PRECOMPILE #endif // PCL_PPF_REGISTRATION_H_ diff --git a/registration/src/ppf_registration.cpp b/registration/src/ppf_registration.cpp index bc68a1fd119..5138f9748c4 100644 --- a/registration/src/ppf_registration.cpp +++ b/registration/src/ppf_registration.cpp @@ -35,12 +35,67 @@ * $Id$ * */ +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include +#include -/** Re-enable these once all of registration is separated into H/HPP correctly. */ -//#include -//#include -//#include -//#include -// -//PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)); -// +PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)); +#endif // PCL_NO_PRECOMPILE + +void +pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud::ConstPtr feature_cloud) +{ + // Discretize the feature cloud and insert it in the hash map + feature_hash_map_->clear (); + unsigned int n = static_cast (sqrt (static_cast (feature_cloud->points.size ()))); + int d1, d2, d3, d4; + max_dist_ = -1.0; + alpha_m_.resize (n); + for (size_t i = 0; i < n; ++i) + { + std::vector alpha_m_row (n); + for (size_t j = 0; j < n; ++j) + { + d1 = static_cast (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_)); + d2 = static_cast (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_)); + d3 = static_cast (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_)); + d4 = static_cast (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_)); + feature_hash_map_->insert (std::pair > (HashKeyStruct (d1, d2, d3, d4), std::pair (i, j))); + alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m; + + if (max_dist_ < feature_cloud->points[i*n + j].f4) + max_dist_ = feature_cloud->points[i*n + j].f4; + } + alpha_m_[i] = alpha_m_row; + } + + internals_initialized_ = true; +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4, + std::vector > &indices) +{ + if (!internals_initialized_) + { + PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n"); + return; + } + + int d1 = static_cast (floor (f1 / angle_discretization_step_)), + d2 = static_cast (floor (f2 / angle_discretization_step_)), + d3 = static_cast (floor (f3 / angle_discretization_step_)), + d4 = static_cast (floor (f4 / distance_discretization_step_)); + + indices.clear (); + HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4); + std::pair map_iterator_pair = feature_hash_map_->equal_range (key); + for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first) + indices.push_back (std::pair (map_iterator_pair.first->second.first, + map_iterator_pair.first->second.second)); +}