diff --git a/registration/include/pcl/registration/impl/ppf_registration.hpp b/registration/include/pcl/registration/impl/ppf_registration.hpp index f2225cf2d7d..fc752a48e1f 100644 --- a/registration/include/pcl/registration/impl/ppf_registration.hpp +++ b/registration/include/pcl/registration/impl/ppf_registration.hpp @@ -105,6 +105,16 @@ pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, f } +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::PPFRegistration::setInputTarget (const PointCloudTargetConstPtr &cloud) +{ + Registration::setInputTarget (cloud); + + scene_search_tree_ = typename pcl::KdTreeFLANN::Ptr (new pcl::KdTreeFLANN); + scene_search_tree_->setInputCloud (target_); +} + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::PPFRegistration::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) @@ -148,7 +158,7 @@ pcl::PPFRegistration::computeTransformation (PointClou // For every other point in the scene => now have pair (s_r, s_i) fixed std::vector indices; std::vector distances; - tree_->radiusSearch (target_->points[scene_reference_index], + scene_search_tree_->radiusSearch (target_->points[scene_reference_index], search_method_->getModelDiameter () /2, indices, distances); diff --git a/registration/include/pcl/registration/ppf_registration.h b/registration/include/pcl/registration/ppf_registration.h index 789f0016b00..2560ec6fd63 100644 --- a/registration/include/pcl/registration/ppf_registration.h +++ b/registration/include/pcl/registration/ppf_registration.h @@ -234,6 +234,12 @@ namespace pcl inline PPFHashMapSearch::Ptr getSearchMethod () { return search_method_; } + /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) + * \param cloud the input point cloud target + */ + void + setInputTarget (const PointCloudTargetConstPtr &cloud); + private: /** \brief Method that calculates the transformation between the input_ and target_ point clouds, based on the PPF features */ @@ -251,6 +257,9 @@ namespace pcl * poses are considered to be in the same cluster (for the clustering phase of the algorithm) */ float clustering_position_diff_threshold_, clustering_rotation_diff_threshold_; + /** \brief use a kd-tree with range searches of range max_dist to skip an O(N) pass through the point cloud */ + typename pcl::KdTreeFLANN::Ptr scene_search_tree_; + /** \brief static method used for the std::sort function to order two PoseWithVotes * instances by their number of votes*/ static bool