From 94b5bcae02a657662c3ef381e5dc1dd970358645 Mon Sep 17 00:00:00 2001 From: Ramzi Sabra Date: Wed, 1 Jun 2022 05:00:39 +0300 Subject: [PATCH 1/5] added max leaf size to KdTree interface; implemented max leaf size for KdTreeFLANN added maximum leaf node size getter to KdTree base class --- .../include/pcl/kdtree/impl/kdtree_flann.hpp | 6 ++--- kdtree/include/pcl/kdtree/kdtree.h | 25 ++++++++++++++++--- kdtree/include/pcl/kdtree/kdtree_flann.h | 10 +++++++- 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp index d276dee8a17..150b10be470 100644 --- a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp +++ b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp @@ -46,8 +46,8 @@ /////////////////////////////////////////////////////////////////////////////////////////// template -pcl::KdTreeFLANN::KdTreeFLANN (bool sorted) - : pcl::KdTree (sorted) +pcl::KdTreeFLANN::KdTreeFLANN (bool sorted, int max_leaf_size) + : pcl::KdTree (sorted, max_leaf_size) , flann_index_ () , identity_mapping_ (false) , dim_ (0), total_nr_points_ (0) @@ -133,7 +133,7 @@ pcl::KdTreeFLANN::setInputCloud (const PointCloudConstPtr &cloud, flann_index_.reset (new FLANNIndex (::flann::Matrix (cloud_.get (), index_mapping_.size (), dim_), - ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf + ::flann::KDTreeSingleIndexParams (max_leaf_size_))); flann_index_->buildIndex (); } diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h index 82baf32a49c..8c4cb3626ba 100644 --- a/kdtree/include/pcl/kdtree/kdtree.h +++ b/kdtree/include/pcl/kdtree/kdtree.h @@ -70,10 +70,11 @@ namespace pcl /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. Set to 15 by default. */ - KdTree (bool sorted = true) : input_(), - epsilon_(0.0f), min_pts_(1), sorted_(sorted), - point_representation_ (new DefaultPointRepresentation) + KdTree (bool sorted = true, int max_leaf_size = 15) : input_(), + max_leaf_size_(max_leaf_size), epsilon_(0.0f), min_pts_(1), sorted_(sorted), + point_representation_ (new DefaultPointRepresentation) { }; @@ -299,6 +300,21 @@ namespace pcl return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); } + /** \brief Set the maximum leaf node size. + * \param[in] max_leaf_size maximum leaf node size + */ + virtual inline void + setMaxLeafSize(int max_leaf_size) + { + max_leaf_size_ = max_leaf_size; + } + + /** \brief Get the maximum leaf node size. */ + inline int getMaxLeafSize() const + { + return (max_leaf_size_); + } + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. * \param[in] eps precision (error bound) for nearest neighbors searches */ @@ -338,6 +354,9 @@ namespace pcl /** \brief A pointer to the vector of point indices to use. */ IndicesConstPtr indices_; + /** \brief Maximum leaf node size */ + int max_leaf_size_; + /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ float epsilon_; diff --git a/kdtree/include/pcl/kdtree/kdtree_flann.h b/kdtree/include/pcl/kdtree/kdtree_flann.h index e41b96c7a29..fc64c41b2c2 100644 --- a/kdtree/include/pcl/kdtree/kdtree_flann.h +++ b/kdtree/include/pcl/kdtree/kdtree_flann.h @@ -133,6 +133,7 @@ class KdTreeFLANN : public pcl::KdTree { public: using KdTree::input_; using KdTree::indices_; + using KdTree::max_leaf_size_; using KdTree::epsilon_; using KdTree::sorted_; using KdTree::point_representation_; @@ -154,10 +155,11 @@ class KdTreeFLANN : public pcl::KdTree { /** \brief Default Constructor for KdTreeFLANN. * \param[in] sorted set to true if the application that the tree will be used for * requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. Set to 15 by default. * * By setting sorted to false, the \ref radiusSearch operations will be faster. */ - KdTreeFLANN(bool sorted = true); + KdTreeFLANN(bool sorted = true, int max_leaf_size = 15); /** \brief Copy constructor * \param[in] k the tree to copy into this @@ -182,6 +184,12 @@ class KdTreeFLANN : public pcl::KdTree { return (*this); } + /** \brief Set the maximum leaf node size. + * \param[in] max_leaf_size maximum leaf node size + */ + void + setMaxLeafSize(int max_leaf_size) override; + /** \brief Set the search epsilon precision (error bound) for nearest neighbors * searches. \param[in] eps precision (error bound) for nearest neighbors searches */ From 9ca205773df02b38cf1b17144afabad1276d0785 Mon Sep 17 00:00:00 2001 From: Ramzi Sabra Date: Tue, 31 May 2022 23:57:32 +0300 Subject: [PATCH 2/5] added KdTreeNanoflann; adjusted kdtree tests --- kdtree/CMakeLists.txt | 28 +- .../pcl/kdtree/impl/kdtree_nanoflann.hpp | 402 ++++++++++++++++++ kdtree/include/pcl/kdtree/kdtree_nanoflann.h | 305 +++++++++++++ kdtree/src/kdtree_nanoflann.cpp | 47 ++ test/kdtree/CMakeLists.txt | 4 + test/kdtree/test_kdtree.cpp | 112 +++-- 6 files changed, 855 insertions(+), 43 deletions(-) create mode 100644 kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp create mode 100644 kdtree/include/pcl/kdtree/kdtree_nanoflann.h create mode 100644 kdtree/src/kdtree_nanoflann.cpp diff --git a/kdtree/CMakeLists.txt b/kdtree/CMakeLists.txt index a5a0fb9b8ec..cd9804b57d4 100644 --- a/kdtree/CMakeLists.txt +++ b/kdtree/CMakeLists.txt @@ -1,10 +1,21 @@ set(SUBSYS_NAME kdtree) set(SUBSYS_DESC "Point cloud kd-tree library") set(SUBSYS_DEPS common) +set(EXT_DEPS flann) + +option(BUILD_kdtree_nanoflann "Build kd-tree making use of nanoflann" ON) + +if(BUILD_kdtree_nanoflann) + find_package(nanoflann 1.4.2) + if (nanoflann_FOUND) + set(HAVE_nanoflann ON PARENT_SCOPE) + endif() +endif() set(build TRUE) + PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS flann) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) PCL_ADD_DOC("${SUBSYS_NAME}") @@ -27,11 +38,22 @@ set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/kdtree_flann.hpp" ) +set(libs + pcl_common + FLANN::FLANN +) + +if(HAVE_nanoflann) + list(APPEND srcs src/kdtree_nanoflann.cpp) + list(APPEND incs include/pcl/${SUBSYS_NAME}/kdtree_nanoflann.h) + list(APPEND impl_incs include/pcl/${SUBSYS_NAME}/impl/kdtree_nanoflann.hpp) + list(APPEND libs nanoflann::nanoflann) +endif() + set(LIB_NAME "pcl_${SUBSYS_NAME}") include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs}) -target_link_libraries("${LIB_NAME}" pcl_common FLANN::FLANN) -set(EXT_DEPS flann) +target_link_libraries("${LIB_NAME}" ${libs}) PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) # Install include files diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp new file mode 100644 index 00000000000..8042f02926f --- /dev/null +++ b/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp @@ -0,0 +1,402 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * kdtree_nanoflann.hpp + * Adapted from: kdtree_flann.hpp + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#ifndef PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ +#define PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::KdTreeNanoflann::KdTreeNanoflann (bool sorted, int max_leaf_size) + : pcl::KdTree (sorted, max_leaf_size) + , nanoflann_index_ () + , identity_mapping_ (false) + , dim_ (0), total_nr_points_ (0) + , param_radius_ (::nanoflann::SearchParams (-1, epsilon_, sorted)) +{} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::KdTreeNanoflann::KdTreeNanoflann (const KdTreeNanoflann &k) + : pcl::KdTree (false) + , nanoflann_index_ () + , identity_mapping_ (false) + , dim_ (0), total_nr_points_ (0) + , param_radius_ (::nanoflann::SearchParams (-1, epsilon_, false)) +{ + *this = k; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::setMaxLeafSize (int max_leaf_size) +{ + max_leaf_size_ = max_leaf_size; + + if (nanoflann_index_ != nullptr) + { + nanoflann_index_.reset(new NanoflannIndex(dim_, cloud_mat_, max_leaf_size_)); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::setEpsilon (float eps) +{ + epsilon_ = eps; + param_radius_ = ::nanoflann::SearchParams (-1 , epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::setSortedResults (bool sorted) +{ + sorted_ = sorted; + param_radius_ = ::nanoflann::SearchParams (-1, epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) +{ + cleanup (); // Perform an automatic cleanup of structures + + epsilon_ = 0.0f; // default error bound value + dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz + + input_ = cloud; + indices_ = indices; + + // Allocate enough data + if (!input_) + { + PCL_ERROR ("[pcl::KdTreeNanoflann::setInputCloud] Invalid input!\n"); + return; + } + if (indices != nullptr) + { + convertCloudToArray (*input_, *indices_); + } + else + { + convertCloudToArray (*input_); + } + total_nr_points_ = static_cast (index_mapping_.size ()); + if (total_nr_points_ == 0) + { + PCL_ERROR ("[pcl::KdTreeNanoflann::setInputCloud] Cannot create a KDTree with an empty input cloud!\n"); + return; + } + + nanoflann_index_.reset(new NanoflannIndex(dim_, cloud_mat_, max_leaf_size_)); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl { +namespace nanoflann { +namespace detail { +// Replace using constexpr in C++17 +template +int +knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k) +{ + Eigen::Vector k_indices_long_mat(k); + int result = index.index->knnSearch(query.data(), k, k_indices_long_mat.data(), dists.data()); + + // Wrap k_indices vector (no data allocation) + Eigen::Map> k_indices_mat(&k_indices[0], k); + k_indices_mat = k_indices_long_mat.template cast(); + return result; +} +} // namespace detail +template +int +knn_search(const Index& index, + const Query& query, + Indices& indices, + Distances& dists, + unsigned int k) +{ + return detail::knn_search(index, query, indices, dists, k); +} +} // namespace nanoflann +} // namespace pcl + +template int +pcl::KdTreeNanoflann::nearestKSearch (const PointT &point, unsigned int k, + Indices &k_indices, + std::vector &k_distances) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); + + if (k > total_nr_points_) + k = total_nr_points_; + + k_indices.resize (k); + k_distances.resize (k); + + if (k==0) + return 0; + + std::vector query (dim_); + point_representation_->vectorize (static_cast (point), query); + + Eigen::Map> query_mat (&query[0], dim_); + + // Wrap the k_distances vector (no data copy) + Eigen::Map> k_distances_mat (&k_distances[0], k); + + pcl::nanoflann::knn_search(*nanoflann_index_, + query_mat, + k_indices, + k_distances_mat, + k); + + // Do mapping to original point cloud + if (!identity_mapping_) + { + for (std::size_t i = 0; i < static_cast (k); ++i) + { + auto& neighbor_index = k_indices[i]; + neighbor_index = index_mapping_[neighbor_index]; + } + } + + return (k); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl { +namespace nanoflann { +namespace detail { +// Replace using constexpr in C++17 +template +int +radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params) +{ + std::vector> indices_dists(1); + int neighbors_in_radius = index.index->radiusSearch(query.data(), radius, indices_dists, params); + + k_indices.clear(); + k_indices.reserve(indices_dists.size()); + + dists.clear(); + dists.reserve(indices_dists.size()); + + for (const auto& index_dist : indices_dists) + { + const long& index = index_dist.first; + const float& dist = index_dist.second; + + k_indices.push_back(index); + dists.push_back(dist); + } + + return neighbors_in_radius; +} +} // namespace detail +template +int +radius_search(const Index& index, + const Query& query, + Indices& indices, + Distances& dists, + float radius, + const SearchParams& params) +{ + return detail::radius_search( + index, query, indices, dists, radius, params); +} +} // namespace nanoflann +} // namespace pcl + +template int +pcl::KdTreeNanoflann::radiusSearch (const PointT &point, double radius, Indices &k_indices, + std::vector &k_sqr_dists, unsigned int max_nn) const +{ + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); + + std::vector query (dim_); + point_representation_->vectorize (static_cast (point), query); + + // Has max_nn been set properly? + if (max_nn == 0 || max_nn > total_nr_points_) + max_nn = total_nr_points_; + + ::nanoflann::SearchParams params (param_radius_); + + Eigen::Map> query_mat (&query[0], dim_); + int neighbors_in_radius = pcl::nanoflann::radius_search(*nanoflann_index_, + query_mat, + k_indices, + k_sqr_dists, + static_cast(radius * radius), + params); + + // testing + if (k_indices.size() > max_nn) + { + k_indices.resize(max_nn); + k_sqr_dists.resize(max_nn); + } + + // Do mapping to original point cloud + if (!identity_mapping_) + { + for (int i = 0; i < neighbors_in_radius; ++i) + { + auto& neighbor_index = k_indices[i]; + neighbor_index = index_mapping_[neighbor_index]; + } + } + + return (neighbors_in_radius); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::cleanup () +{ + // Data array cleanup + index_mapping_.clear (); + + if (indices_) + indices_.reset (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::convertCloudToArray (const PointCloud &cloud) +{ + // No point in doing anything if the cloud is empty + if (cloud.empty ()) + { + cloud_mat_.resize(0, dim_); + return; + } + + const auto original_no_of_points = cloud.size (); + + cloud_mat_.resize(original_no_of_points, dim_); + + index_mapping_.reserve (original_no_of_points); + identity_mapping_ = true; + + for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud[cloud_index])) + { + identity_mapping_ = false; + continue; + } + + index_mapping_.push_back (cloud_index); + + auto point_vec = cloud_mat_.row(cloud_index); + point_representation_->vectorize (cloud[cloud_index], point_vec); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeNanoflann::convertCloudToArray (const PointCloud &cloud, const Indices &indices) +{ + // No point in doing anything if the cloud is empty + if (cloud.empty ()) + { + cloud_mat_.resize(0, dim_); + return; + } + + int original_no_of_points = static_cast (indices.size ()); + + cloud_mat_.resize(original_no_of_points, dim_); + + index_mapping_.reserve (original_no_of_points); + // its a subcloud -> false + // true only identity: + // - indices size equals cloud size + // - indices only contain values between 0 and cloud.size - 1 + // - no index is multiple times in the list + // => index is complete + // But we can not guarantee that => identity_mapping_ = false + identity_mapping_ = false; + + size_t row_idx = 0; + + for (const auto &index : indices) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud[index])) + continue; + + // map from 0 - N -> indices [0] - indices [N] + index_mapping_.push_back (index); // If the returned index should be for the indices vector + + auto point_vec = cloud_mat_.row(row_idx++); + point_representation_->vectorize (cloud[index], point_vec); + } +} + +#define PCL_INSTANTIATE_KdTreeNanoflann(T) template class PCL_EXPORTS pcl::KdTreeNanoflann; + +#endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ + diff --git a/kdtree/include/pcl/kdtree/kdtree_nanoflann.h b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h new file mode 100644 index 00000000000..ef9d74b1562 --- /dev/null +++ b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * kdtree_nanoflann.h + * Adapted from: kdtree_flann.h + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#pragma once + +#include +#include + +#include + +namespace pcl { +namespace nanoflann { +/** + * @brief Compatibility template function to allow use of various types of indices with + * nanoflann + * @details Template is used for all params to not constrain any nanoflann side capability + * @param[in,out] index A index searcher, of type + * ::nanoflann::KDTreeSingleIndexAdaptor or similar, where Dist is a + * template for computing distance between 2 points + * @param[in] query An Eigen::MatrixXf or compatible matrix representation of the + * query point + * @param[out] indices Indices found in radius + * @param[out] dists Computed distance matrix + * @param[in] radius Threshold for consideration + * @param[in] params Any parameters to pass to the radius_search call + */ +template +int +radius_search(const NanoflannIndex& index, + const Query& query, + Indices& indices, + Distances& dists, + float radius, + const SearchParams& params); + +/** + * @brief Compatibility template function to allow use of various types of indices with + * nanoflann + * @details Template is used for all params to not constrain any nanoflann side capability + * @param[in,out] index A index searcher, of type + * ::nanoflann::KDTreeSingleIndexAdaptor or similar, where Dist is a + * template for computing distance between 2 points + * @param[in] query An Eigen::MatrixXf or compatible matrix representation of the + * query point + * @param[out] indices Neighboring k indices found + * @param[out] dists Computed distance matrix + * @param[in] k Number of neighbors to search for + * @param[in] params Any parameters to pass to the knn_search call + */ +template +int +knn_search(const NanoflannIndex& index, + const Query& query, + Indices& indices, + Distances& dists, + unsigned int k, + const SearchParams& params); +} // namespace nanoflann + +/** \brief KdTreeNanoflann is a generic type of 3D spatial locator using kd-tree structures. + * The class is making use of the nanoflann project by Jose Luis Blanco-Claraco. + * + * \author Ramzi Sabra + * \ingroup kdtree + */ +template +class KdTreeNanoflann : public pcl::KdTree { +public: + using KdTree::input_; + using KdTree::indices_; + using KdTree::max_leaf_size_; + using KdTree::epsilon_; + using KdTree::sorted_; + using KdTree::point_representation_; + using KdTree::nearestKSearch; + using KdTree::radiusSearch; + + using PointCloud = typename KdTree::PointCloud; + using PointCloudConstPtr = typename KdTree::PointCloudConstPtr; + + using IndicesPtr = shared_ptr; + using IndicesConstPtr = shared_ptr; + + using Matrix = Eigen::Matrix; + using NanoflannIndex = ::nanoflann::KDTreeEigenMatrixAdaptor; + + using Tree = KdTreeNanoflann; + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Default Constructor for KdTreeNanoflann. + * \param[in] sorted set to true if the application that the tree will be used for + * requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. Set to 15 by default. + * + * By setting sorted to false, \ref radiusSearch operations will be faster. + */ + KdTreeNanoflann(bool sorted = true, int max_leaf_size = 15); + + /** \brief Copy constructor + * \param[in] k the tree to copy into this + */ + KdTreeNanoflann(const Tree& k); + + /** \brief Copy operator + * \param[in] k the tree to copy into this + */ + inline Tree& + operator=(const Tree& k) + { + KdTree::operator=(k); + index_mapping_ = k.index_mapping_; + identity_mapping_ = k.identity_mapping_; + dim_ = k.dim_; + total_nr_points_ = k.total_nr_points_; + param_radius_ = k.param_radius_; + + if (k.nanoflann_index_ != nullptr) + { + cloud_mat_ = k.cloud_mat_; + nanoflann_index_.reset(new NanoflannIndex(dim_, cloud_mat_, max_leaf_size_)); + } + + return (*this); + } + + /** \brief Set the maximum leaf node size. + * \param[in] max_leaf_size maximum leaf node size + */ + void + setMaxLeafSize(int max_leaf_size) override; + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors + * searches. \param[in] eps precision (error bound) for nearest neighbors searches + */ + void + setEpsilon(float eps) override; + + void + setSortedResults(bool sorted); + + inline Ptr + makeShared() + { + return Ptr(new Tree(*this)); + } + + /** \brief Destructor for KdTreeFLANN. + * Deletes all allocated data arrays and destroys the kd-tree structures. + */ + ~KdTreeNanoflann() { cleanup(); } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud - if + * NULL the whole cloud is used + */ + void + setInputCloud(const PointCloudConstPtr& cloud, + const IndicesConstPtr& indices = IndicesConstPtr()) override; + + /** \brief Search for k-nearest neighbors for the given query point. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] point a given \a valid (i.e., finite) query point + * \param[in] k the number of neighbors to search for + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + int + nearestKSearch(const PointT& point, + unsigned int k, + Indices& k_indices, + std::vector& k_sqr_distances) const override; + + /** \brief Search for all the nearest neighbors of the query point in a given radius. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] point a given \a valid (i.e., finite) query point + * \param[in] radius the radius of the sphere bounding all of p_q's neighbors + * \param[out] k_indices the resultant indices of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances to the neighboring + * points \param[in] max_nn if given, bounds the maximum returned neighbors to this + * value. If \a max_nn is set to 0 or to a number higher than the number of points in + * the input cloud, all neighbors in \a radius will be returned. Note that nanoflann, + * internally, fetches all neighbors in radius regardless of the value of \a max_nn. + * \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + int + radiusSearch(const PointT& point, + double radius, + Indices& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const override; + +private: + /** \brief Internal cleanup method. */ + void + cleanup(); + + /** \brief Converts a PointCloud to an Eigen row-major matrix of floats. + * \param cloud the PointCloud + */ + void + convertCloudToArray(const PointCloud& cloud); + + /** \brief Converts a PointCloud with a given set of indices to an Eigen row-major + * matrix of floats. \param[in] cloud the PointCloud data \param[in] indices the point + * cloud indices + */ + void + convertCloudToArray(const PointCloud& cloud, const Indices& indices); + + /** \brief Class getName method. */ + std::string + getName() const override + { + return ("KdTreeNanoflann"); + } + + /** \brief A FLANN index object. */ + std::shared_ptr nanoflann_index_; + + /** \brief cloud dataset matrix. */ + Matrix cloud_mat_; + + /** \brief Mapping between internal and external indices. */ + std::vector index_mapping_; + + /** \brief Whether the mapping between internal and external indices is identity */ + bool identity_mapping_; + + /** \brief Tree dimensionality (i.e. the number of dimensions per point). */ + int dim_; + + /** \brief The total size of the data (either equal to the number of points in the + * input cloud or to the number of indices - if passed). */ + uindex_t total_nr_points_; + + /** \brief The kd-tree search parameters for radius search. */ + ::nanoflann::SearchParams param_radius_; +}; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/kdtree/src/kdtree_nanoflann.cpp b/kdtree/src/kdtree_nanoflann.cpp new file mode 100644 index 00000000000..0487cf253e8 --- /dev/null +++ b/kdtree/src/kdtree_nanoflann.cpp @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +PCL_INSTANTIATE(KdTreeNanoflann, PCL_POINT_TYPES) +#endif // PCL_NO_PRECOMPILE + diff --git a/test/kdtree/CMakeLists.txt b/test/kdtree/CMakeLists.txt index b8391022d77..2d4d4daec9a 100644 --- a/test/kdtree/CMakeLists.txt +++ b/test/kdtree/CMakeLists.txt @@ -16,3 +16,7 @@ PCL_ADD_TEST (kdtree_kdtree test_kdtree FILES test_kdtree.cpp LINK_WITH pcl_gtest pcl_kdtree pcl_io pcl_common ARGUMENTS "${PCL_SOURCE_DIR}/test/sac_plane_test.pcd" "${PCL_SOURCE_DIR}/test/kdtree/kdtree_unit_test_results.xml") + +if(HAVE_nanoflann) + target_compile_definitions(test_kdtree PRIVATE PCL_TEST_KDTREE_NANOFLANN) +endif() diff --git a/test/kdtree/test_kdtree.cpp b/test/kdtree/test_kdtree.cpp index f0baf755c1e..bc8629b8c09 100644 --- a/test/kdtree/test_kdtree.cpp +++ b/test/kdtree/test_kdtree.cpp @@ -38,6 +38,10 @@ #include +#ifdef PCL_TEST_KDTREE_NANOFLANN +#include +#endif + #include #include #include @@ -65,33 +69,53 @@ struct MyPoint : public PointXYZ MyPoint (float x, float y, float z) {this->x=x; this->y=y; this->z=z;} }; -PointCloud cloud, cloud_big; - -void -init () +template +class PCLKdTreeTestFixture : public ::testing::Test { - float resolution = 0.1f; - for (float z = -0.5f; z <= 0.5f; z += resolution) - for (float y = -0.5f; y <= 0.5f; y += resolution) - for (float x = -0.5f; x <= 0.5f; x += resolution) - cloud.emplace_back(x, y, z); - cloud.width = cloud.size (); - cloud.height = 1; - - cloud_big.width = 640; - cloud_big.height = 480; - srand (static_cast (time (nullptr))); - // Randomly create a new point cloud - for (std::size_t i = 0; i < cloud_big.width * cloud_big.height; ++i) - cloud_big.emplace_back(static_cast (1024 * rand () / (RAND_MAX + 1.0)), - static_cast (1024 * rand () / (RAND_MAX + 1.0)), - static_cast (1024 * rand () / (RAND_MAX + 1.0))); -} + public: + using TreeMyPoint = std::tuple_element_t<0, TupleType>; + using TreePointXYZ = std::tuple_element_t<1, TupleType>; + + PointCloud cloud_, cloud_big_; + + PCLKdTreeTestFixture() + { + float resolution = 0.1f; + for (float z = -0.5f; z <= 0.5f; z += resolution) + for (float y = -0.5f; y <= 0.5f; y += resolution) + for (float x = -0.5f; x <= 0.5f; x += resolution) + cloud_.emplace_back(x, y, z); + cloud_.width = cloud_.size (); + cloud_.height = 1; + + cloud_big_.width = 640; + cloud_big_.height = 480; + srand (static_cast (time (nullptr))); + // Randomly create a new point cloud + for (std::size_t i = 0; i < cloud_big_.width * cloud_big_.height; ++i) + cloud_big_.emplace_back(static_cast (1024 * rand () / (RAND_MAX + 1.0)), + static_cast (1024 * rand () / (RAND_MAX + 1.0)), + static_cast (1024 * rand () / (RAND_MAX + 1.0))); + } +}; + +using KdTreeTestTypes = ::testing::Types< + std::tuple, KdTreeFLANN> + #ifdef PCL_TEST_KDTREE_NANOFLANN + , std::tuple, KdTreeNanoflann> + #endif +>; +TYPED_TEST_SUITE(PCLKdTreeTestFixture, KdTreeTestTypes); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, KdTreeFLANN_radiusSearch) +TYPED_TEST (PCLKdTreeTestFixture, KdTree_radiusSearch) { - KdTreeFLANN kdtree; + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + auto& cloud_big = this->cloud_big_; + + Tree kdtree; kdtree.setInputCloud (cloud.makeShared ()); MyPoint test_point(0.0f, 0.0f, 0.0f); double max_dist = 0.15; @@ -123,10 +147,10 @@ TEST (PCL, KdTreeFLANN_radiusSearch) EXPECT_FALSE (error); { - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (cloud_big.makeShared ()); - ScopeTime scopeTime ("FLANN radiusSearch"); + ScopeTime scopeTime ("radiusSearch"); { for (const auto &point : cloud_big.points) kdtree.radiusSearch (point, 0.1, k_indices, k_distances); @@ -134,10 +158,10 @@ TEST (PCL, KdTreeFLANN_radiusSearch) } { - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (cloud_big.makeShared ()); - ScopeTime scopeTime ("FLANN radiusSearch (max neighbors in radius)"); + ScopeTime scopeTime ("radiusSearch (max neighbors in radius)"); { for (const auto &point : cloud_big.points) kdtree.radiusSearch (point, 0.1, k_indices, k_distances, 10); @@ -146,10 +170,10 @@ TEST (PCL, KdTreeFLANN_radiusSearch) { - KdTreeFLANN kdtree (false); + Tree kdtree (false); kdtree.setInputCloud (cloud_big.makeShared ()); - ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)"); + ScopeTime scopeTime ("radiusSearch (unsorted results)"); { for (const auto &point : cloud_big.points) kdtree.radiusSearch (point, 0.1, k_indices, k_distances); @@ -158,9 +182,14 @@ TEST (PCL, KdTreeFLANN_radiusSearch) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, KdTreeFLANN_nearestKSearch) +TYPED_TEST (PCLKdTreeTestFixture, KdTree_nearestKSearch) { - KdTreeFLANN kdtree; + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + auto& cloud_big = this->cloud_big_; + + Tree kdtree; kdtree.setInputCloud (cloud.makeShared ()); MyPoint test_point (0.01f, 0.01f, 0.01f); unsigned int no_of_neighbors = 20; @@ -198,9 +227,9 @@ TEST (PCL, KdTreeFLANN_nearestKSearch) EXPECT_TRUE (ok); } - ScopeTime scopeTime ("FLANN nearestKSearch"); + ScopeTime scopeTime ("nearestKSearch"); { - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (cloud_big.makeShared ()); for (const auto &point : cloud_big.points) kdtree.nearestKSearch (point, no_of_neighbors, k_indices, k_distances); @@ -223,8 +252,10 @@ class MyPointRepresentationXY : public PointRepresentation } }; -TEST (PCL, KdTreeFLANN_setPointRepresentation) +TYPED_TEST (PCLKdTreeTestFixture, KdTree_setPointRepresentation) { + using Tree = typename TestFixture::TreeMyPoint; + PointCloud::Ptr random_cloud (new PointCloud ()); random_cloud->points.emplace_back(86.6f, 42.1f, 92.4f); random_cloud->points.emplace_back(63.1f, 18.4f, 22.3f); @@ -237,7 +268,7 @@ TEST (PCL, KdTreeFLANN_setPointRepresentation) random_cloud->points.emplace_back(14.2f, 95.7f, 34.7f); random_cloud->points.emplace_back( 2.5f, 26.5f, 66.0f); - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (random_cloud); MyPoint p (50.0f, 50.0f, 50.0f); @@ -257,7 +288,7 @@ TEST (PCL, KdTreeFLANN_setPointRepresentation) } // Find k nearest neighbors with a different point representation - KdTreeFLANN::PointRepresentationConstPtr ptrep (new MyPointRepresentationXY); + typename Tree::PointRepresentationConstPtr ptrep (new MyPointRepresentationXY); kdtree.setPointRepresentation (ptrep); kdtree.nearestKSearch (p, k, k_indices, k_distances); for (int i = 0; i < k; ++i) @@ -289,9 +320,11 @@ TEST (PCL, KdTreeFLANN_setPointRepresentation) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, KdTreeFLANN_32_vs_64_bit) +TYPED_TEST (PCLKdTreeTestFixture, KdTree_32_vs_64_bit) { - KdTreeFLANN tree; + using Tree = typename TestFixture::TreePointXYZ; + + Tree tree; tree.setInputCloud (cloud_in); std::vector nn_indices_vector; @@ -334,7 +367,7 @@ main (int argc, char** argv) // Load the standard PCD file from disk if (argc < 3) { - std::cerr << "No test file given. Please download `sac_plane_test.pcd` and 'kdtree_unit_test_results.xml' pass them path to the test." << std::endl; + std::cerr << "No test file given. Please download `sac_plane_test.pcd` and 'kdtree_unit_test_results.xml' pass their paths to the test." << std::endl; return (-1); } @@ -346,7 +379,6 @@ main (int argc, char** argv) testing::InitGoogleTest (&argc, argv); - init (); return (RUN_ALL_TESTS ()); } /* ]--- */ From 98ea16d68524b405acc992f92a9ef21a85e99f14 Mon Sep 17 00:00:00 2001 From: Ramzi Sabra Date: Wed, 1 Jun 2022 19:15:23 +0300 Subject: [PATCH 3/5] added copy and setMaxLeafSize tests to kdtree tests; fixed setMaxLeafSize for KdTreeFLANN --- .../include/pcl/kdtree/impl/kdtree_flann.hpp | 14 ++++ .../pcl/kdtree/impl/kdtree_nanoflann.hpp | 6 +- test/kdtree/test_kdtree.cpp | 83 +++++++++++++++++++ 3 files changed, 99 insertions(+), 4 deletions(-) diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp index 150b10be470..c93ddf02987 100644 --- a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp +++ b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp @@ -79,6 +79,20 @@ pcl::KdTreeFLANN::KdTreeFLANN (const KdTreeFLANN &k) *this = k; } +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::KdTreeFLANN::setMaxLeafSize (int max_leaf_size) +{ + max_leaf_size_ = max_leaf_size; + + if (!input_) return; + flann_index_.reset (new FLANNIndex (::flann::Matrix (cloud_.get (), + index_mapping_.size (), + dim_), + ::flann::KDTreeSingleIndexParams (max_leaf_size_))); + flann_index_->buildIndex (); +} + /////////////////////////////////////////////////////////////////////////////////////////// template void pcl::KdTreeFLANN::setEpsilon (float eps) diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp index 8042f02926f..914e142016a 100644 --- a/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp +++ b/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp @@ -74,10 +74,8 @@ pcl::KdTreeNanoflann::setMaxLeafSize (int max_leaf_size) { max_leaf_size_ = max_leaf_size; - if (nanoflann_index_ != nullptr) - { - nanoflann_index_.reset(new NanoflannIndex(dim_, cloud_mat_, max_leaf_size_)); - } + if (!input_) return; + nanoflann_index_.reset(new NanoflannIndex(dim_, cloud_mat_, max_leaf_size_)); } /////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/kdtree/test_kdtree.cpp b/test/kdtree/test_kdtree.cpp index bc8629b8c09..8f66c388c36 100644 --- a/test/kdtree/test_kdtree.cpp +++ b/test/kdtree/test_kdtree.cpp @@ -56,6 +56,7 @@ #include #include // For debug #include +#include using namespace pcl; @@ -360,6 +361,88 @@ TYPED_TEST (PCLKdTreeTestFixture, KdTree_32_vs_64_bit) } } +TYPED_TEST (PCLKdTreeTestFixture, KdTree_copy) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + + MyPoint test_point (0.01f, 0.01f, 0.01f); + const int k = 10; + const int max_nn = 10; + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + + pcl::Indices k_indices; + std::vector k_distances; + kdtree.nearestKSearch (test_point, k, k_indices, k_distances); + + pcl::Indices indices; + std::vector distances; + kdtree.radiusSearch (test_point, max_nn, indices, distances); + + Tree kdtree_copy = kdtree; + pcl::Indices k_indices_copy; + std::vector k_distances_copy; + kdtree_copy.nearestKSearch (test_point, k, k_indices_copy, k_distances_copy); + + pcl::Indices indices_copy; + std::vector distances_copy; + kdtree_copy.radiusSearch (test_point, max_nn, indices_copy, distances_copy); + + EXPECT_EQ (k_indices, k_indices_copy); + EXPECT_EQ (k_distances, k_distances_copy); + EXPECT_EQ (indices, indices_copy); + EXPECT_EQ (distances, distances_copy); +} + +TYPED_TEST (PCLKdTreeTestFixture, KdTree_setMaxLeafSize) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + + MyPoint test_point (0.01f, 0.01f, 0.01f); + const int k = 10; + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + + pcl::Indices k_indices_default; + std::vector k_distances_default; + kdtree.nearestKSearch (test_point, k, k_indices_default, k_distances_default); + + kdtree.setMaxLeafSize (50); + + pcl::Indices k_indices_changed; + std::vector k_distances_changed; + kdtree.nearestKSearch (test_point, k, k_indices_changed, k_distances_changed); + + std::unordered_set k_indices_set_default, k_indices_set_changed; + + std::copy( + k_indices_default.cbegin(), + k_indices_default.cend(), + std::inserter( + k_indices_set_default, + k_indices_set_default.end() + ) + ); + + std::copy( + k_indices_changed.cbegin(), + k_indices_changed.cend(), + std::inserter( + k_indices_set_changed, + k_indices_set_changed.end() + ) + ); + + EXPECT_EQ (k_indices_set_default, k_indices_set_changed); + EXPECT_EQ (k_distances_default, k_distances_changed); +} + /* ---[ */ int main (int argc, char** argv) From 4fe394bfff8733af9ea1baf62c85945d8ebab3ad Mon Sep 17 00:00:00 2001 From: Ramzi Sabra Date: Thu, 2 Jun 2022 00:02:34 +0300 Subject: [PATCH 4/5] added KdTreeFLANN CUDA with tests fixed template instantiation added compile definition --- CMakeLists.txt | 3 +- cuda/kdtree/CMakeLists.txt | 53 ++ .../pcl/cuda/kdtree/impl/kdtree_flann.hpp | 604 ++++++++++++++++ cuda/kdtree/include/pcl/cuda/kdtree/kdtree.h | 144 ++++ .../include/pcl/cuda/kdtree/kdtree_flann.h | 390 ++++++++++ cuda/kdtree/src/kdtree_flann.cpp | 47 ++ kdtree/CMakeLists.txt | 1 + kdtree/include/pcl/kdtree/base.h | 196 ++++++ kdtree/include/pcl/kdtree/kdtree.h | 148 +--- test/CMakeLists.txt | 1 + test/cuda/CMakeLists.txt | 5 + test/cuda/kdtree/CMakeLists.txt | 19 + test/cuda/kdtree/test_kdtree.cpp | 664 ++++++++++++++++++ 13 files changed, 2138 insertions(+), 137 deletions(-) create mode 100644 cuda/kdtree/CMakeLists.txt create mode 100644 cuda/kdtree/include/pcl/cuda/kdtree/impl/kdtree_flann.hpp create mode 100644 cuda/kdtree/include/pcl/cuda/kdtree/kdtree.h create mode 100644 cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h create mode 100644 cuda/kdtree/src/kdtree_flann.cpp create mode 100644 kdtree/include/pcl/kdtree/base.h create mode 100644 test/cuda/CMakeLists.txt create mode 100644 test/cuda/kdtree/CMakeLists.txt create mode 100644 test/cuda/kdtree/test_kdtree.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fbfaf15f1f..79da3116811 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -319,7 +319,8 @@ find_package(Eigen 3.1 REQUIRED) include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) # FLANN (required) -find_package(FLANN 1.9.1 REQUIRED) +set(flann_DIR ${FLANN_DIR}) # hack for flann::flann_cuda target to be found +find_package(FLANN 1.9.1 REQUIRED flann OPTIONAL_COMPONENTS flann_cuda) if(NOT (${FLANN_LIBRARY_TYPE} MATCHES ${PCL_FLANN_REQUIRED_TYPE}) AND NOT (${PCL_FLANN_REQUIRED_TYPE} MATCHES "DONTCARE")) message(FATAL_ERROR "Flann was selected with ${PCL_FLANN_REQUIRED_TYPE} but found as ${FLANN_LIBRARY_TYPE}") endif() diff --git a/cuda/kdtree/CMakeLists.txt b/cuda/kdtree/CMakeLists.txt new file mode 100644 index 00000000000..676c039f048 --- /dev/null +++ b/cuda/kdtree/CMakeLists.txt @@ -0,0 +1,53 @@ +set(SUBSYS_NAME cuda_kdtree) +set(SUBSYS_PATH cuda/kdtree) +set(SUBSYS_DESC "Point cloud kd-tree CUDA library") +set(SUBSYS_DEPS common cuda_common kdtree) +set(EXT_DEPS flann) + +# ---[ Point Cloud Library - pcl/cuda/kdtree + +if (NOT TARGET flann::flann_cuda) + return() +endif() + +set(build TRUE) + +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +mark_as_advanced("BUILD_${SUBSYS_NAME}") + +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) +PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}") + +if(NOT build) + return() +endif() + +set(srcs + src/kdtree_flann.cpp +) + +set(incs + include/pcl/${SUBSYS_PATH}/kdtree.h + include/pcl/${SUBSYS_PATH}/kdtree_flann.h +) + +set(impl_incs + include/pcl/${SUBSYS_PATH}/impl/kdtree_flann.hpp +) + +set(libs + pcl_kdtree + flann::flann_cuda +) + +set(LIB_NAME "pcl_${SUBSYS_NAME}") +include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") +PCL_CUDA_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs}) +target_link_libraries(${LIB_NAME} ${libs}) +target_compile_definitions(${LIB_NAME} PUBLIC FLANN_USE_CUDA) + +PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) + +# Install include files +PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}" ${incs}) +PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_PATH}/impl" ${impl_incs}) diff --git a/cuda/kdtree/include/pcl/cuda/kdtree/impl/kdtree_flann.hpp b/cuda/kdtree/include/pcl/cuda/kdtree/impl/kdtree_flann.hpp new file mode 100644 index 00000000000..50b96ecf7e4 --- /dev/null +++ b/cuda/kdtree/include/pcl/cuda/kdtree/impl/kdtree_flann.hpp @@ -0,0 +1,604 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * kdtree_flann.hpp + * Adapted from: kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#ifndef PCL_CUDA_KDTREE_KDTREE_IMPL_FLANN_H_ +#define PCL_CUDA_KDTREE_KDTREE_IMPL_FLANN_H_ + +#include + +#include +#include + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +constexpr int pcl::cuda::KdTreeFLANN::dim_; + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::cuda::KdTreeFLANN::KdTreeFLANN (bool sorted, int max_leaf_size) + : pcl::cuda::KdTree (sorted, max_leaf_size) + , flann_index_ () + , identity_mapping_ (false) + , total_nr_points_ (0) + , param_k_ (::flann::SearchParams (-1 , epsilon_)) + , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted)) +{ + if (point_representation_->getNumberOfDimensions() != 3) + throw std::domain_error("Invalid number of dimensions per point (should be 3)."); + + if (!std::is_same::value) { + const auto message = "FLANN is not optimized for current index type. Will incur " + "extra allocations and copy\n"; + if (std::is_same::value) { + PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12 + } + else { + PCL_WARN(message); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::cuda::KdTreeFLANN::KdTreeFLANN (const KdTreeFLANN &k) + : pcl::cuda::KdTree (false) + , flann_index_ () + , identity_mapping_ (false) + , total_nr_points_ (0) + , param_k_ (::flann::SearchParams (-1 , epsilon_)) + , param_radius_ (::flann::SearchParams (-1, epsilon_, false)) +{ + *this = k; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::setMaxLeafSize (int max_leaf_size) +{ + max_leaf_size_ = max_leaf_size; + + if (!input_) return; + ::flann::Matrix mat (cloud_.get (), index_mapping_.size (), dim_); + ::flann::KDTreeCuda3dIndexParams params (max_leaf_size_); + flann_index_.reset (new FLANNIndex (mat, params)); + flann_index_->buildIndex (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::setEpsilon (float eps) +{ + epsilon_ = eps; + param_k_ = ::flann::SearchParams (-1 , epsilon_); + param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::setSortedResults (bool sorted) +{ + sorted_ = sorted; + param_k_ = ::flann::SearchParams (-1, epsilon_); + param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) +{ + cleanup (); // Perform an automatic cleanup of structures + + epsilon_ = 0.0f; // default error bound value + + input_ = cloud; + indices_ = indices; + + // Allocate enough data + if (!input_) + { + PCL_ERROR ("[pcl::cuda::KdTreeFLANN::setInputCloud] Invalid input!\n"); + return; + } + if (indices != nullptr) + { + convertCloudToArray (*input_, *indices_); + } + else + { + convertCloudToArray (*input_); + } + total_nr_points_ = static_cast (index_mapping_.size ()); + if (total_nr_points_ == 0) + { + PCL_ERROR ("[pcl::cuda::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n"); + return; + } + + ::flann::Matrix mat (cloud_.get (), index_mapping_.size (), dim_); + ::flann::KDTreeCuda3dIndexParams params (max_leaf_size_); + flann_index_.reset (new FLANNIndex (mat, params)); + flann_index_->buildIndex (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::setPointRepresentation(const PointRepresentationConstPtr &point_representation) +{ + if (point_representation->getNumberOfDimensions() != 3) + throw std::domain_error("Invalid number of dimensions per point (should be 3)."); + Base::setPointRepresentation(point_representation); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl { +namespace cuda { +namespace flann { +namespace detail { +using IndexMatrix = Eigen::Matrix; +// Replace using constexpr in C++17 +template = true> +int +knn_search(A& index, + B& queries, + IndexMatrix& k_indices, + ::flann::Matrix& dists, + unsigned int k, + F& params) +{ + ::flann::Matrix k_indices_wrapper (k_indices.data(), k_indices.rows(), k); + return index.knnSearch(queries, k_indices_wrapper, dists, k, params); +} + +template = true> +int +knn_search(A& index, + B& queries, + IndexMatrix& k_indices, + ::flann::Matrix& dists, + unsigned int k, + F& params) +{ + Eigen::Matrix indices (queries.rows, k); + ::flann::Matrix indices_wrapper (indices.data(), queries.rows, k); + // flann will resize accordingly + auto ret = index.knnSearch(queries, indices_wrapper, dists, k, params); + k_indices = indices.template cast(); + return ret; +} +} // namespace detail +template +int +knn_search(const FlannIndex& index, + const Queries& queries, + Indices& indices, + Distances& dists, + unsigned int k, + const SearchParams& params) +{ + return detail::knn_search(index, queries, indices, dists, k, params); +} +} // namespace flann +} // namespace cuda +} // namespace pcl + +template int +pcl::cuda::KdTreeFLANN::nearestKSearch(const PointVector& points, unsigned int k, + IndexMatrix& k_indices_mat, + DistanceMatrix& k_sqr_distances_mat) const +{ + using QueryMatrix = Eigen::Matrix; + +#if DEBUG + for (const PointT& point : points) + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); +#endif + + if (k > total_nr_points_) + k = total_nr_points_; + + k_indices_mat.resize (points.size(), k); + k_sqr_distances_mat.resize (points.size(), k); + + if (k==0) + return 0; + + QueryMatrix queries_mat (points.size(), dim_); + + auto point_itr = points.cbegin(); + auto query_itr = queries_mat.rowwise().begin(); + + for (; point_itr != points.cend(); ++point_itr, ++query_itr) + { + const PointT& point = *point_itr; + auto query = *query_itr; + point_representation_->vectorize (point, query); + } + + ::flann::Matrix queries_mat_wrapper (queries_mat.data(), points.size(), dim_); + ::flann::Matrix k_distances_mat_wrapper (k_sqr_distances_mat.data(), points.size(), k); + + pcl::cuda::flann::knn_search(*flann_index_, + queries_mat_wrapper, + k_indices_mat, + k_distances_mat_wrapper, + k, + param_k_); + + // Do mapping to original point cloud + if (!identity_mapping_) + { + std::transform( + k_indices_mat.reshaped().cbegin(), + k_indices_mat.reshaped().cend(), + k_indices_mat.reshaped().begin(), + [this](const index_t& neighbor_index) + { return index_mapping_[neighbor_index]; } + ); + } + + return (k); +} + +template int +pcl::cuda::KdTreeFLANN::nearestKSearch (const PointVector& points, unsigned int k, + IndicesVector& k_indices_vec, + DistancesVector& k_sqr_distances_vec) const +{ + if (k > total_nr_points_) + k = total_nr_points_; + + IndexMatrix k_indices_mat; + DistanceMatrix k_sqr_distances_mat; + + nearestKSearch(points, k, k_indices_mat, k_sqr_distances_mat); + + k_indices_vec.resize(points.size()); + k_sqr_distances_vec.resize(points.size()); + + for (Indices& k_indices : k_indices_vec) + k_indices.resize (k); + for (std::vector& k_sqr_distances : k_sqr_distances_vec) + k_sqr_distances.resize (k); + + // copy indices from matrix to vector + auto k_indices_row_itr = k_indices_mat.rowwise().cbegin(); + auto k_indices_itr = k_indices_vec.begin(); + + for (; k_indices_itr != k_indices_vec.end(); ++k_indices_row_itr, ++k_indices_itr) + { + auto k_indices_row = *k_indices_row_itr; + auto& k_indices = *k_indices_itr; + std::copy(k_indices_row.cbegin(), k_indices_row.cend(), k_indices.begin()); + } + + // copy square distances from matrix to vector + auto k_distances_row_itr = k_sqr_distances_mat.rowwise().cbegin(); + auto k_distances_itr = k_sqr_distances_vec.begin(); + + for (; k_distances_itr != k_sqr_distances_vec.end(); ++k_distances_row_itr, ++k_distances_itr) + { + auto k_distances_row = *k_distances_row_itr; + auto& k_distances = *k_distances_itr; + std::copy(k_distances_row.cbegin(), k_distances_row.cend(), k_distances.begin()); + } + + return (k); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +namespace pcl { +namespace cuda { +namespace flann { +namespace detail { +// Replace using constexpr in C++17 +template = true> +int +radius_search(A& index, + B& queries, + IndexMatrix& indices, + ::flann::Matrix& sqr_distances, + float radius, + F& params) +{ + ::flann::Matrix indices_wrapper (indices.data(), indices.rows(), indices.cols()); + return index.radiusSearch(queries, indices_wrapper, sqr_distances, radius, params); +} + +template = true> +int +radius_search(A& index, + B& queries, + IndexMatrix& indices, + ::flann::Matrix& sqr_distances, + float radius, + F& params) +{ + Eigen::Matrix indices_int (queries.rows, params.max_neighbors); + ::flann::Matrix indices_wrapper (indices_int.data(), queries.rows, params.max_neighbors); + // flann will resize accordingly + int result = index.radiusSearch(queries, indices_wrapper, sqr_distances, radius, params); + indices = indices_int.template cast(); + return result; +} +} // namespace detail +template +int +radius_search(const FlannIndex& index, + const Queries& queries, + Indices& indices, + Distances& sqr_distances, + float radius, + const SearchParams& params) +{ + return detail::radius_search( + index, queries, indices, sqr_distances, radius, params); +} +} // namespace flann +} // namespace cuda +} // namespace pcl + +template int +pcl::cuda::KdTreeFLANN::radiusSearch (const PointVector& points, + double radius, + IndexMatrix &indices_mat, + DistanceMatrix& sqr_distances_mat, + unsigned int max_nn) const +{ + using QueryMatrix = Eigen::Matrix; + +# if DEBUG + for (const PointT& point : points) + assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); +#endif + + // Has max_nn been set properly? + if (max_nn == 0 || max_nn > total_nr_points_) + max_nn = total_nr_points_; + + ::flann::SearchParams params (param_radius_); + if (max_nn == total_nr_points_) + params.max_neighbors = -1; // return all neighbors in radius + else + params.max_neighbors = max_nn; + + QueryMatrix queries_mat (points.size(), dim_); + + auto point_itr = points.cbegin(); + auto query_itr = queries_mat.rowwise().begin(); + + for (; point_itr != points.cend(); ++point_itr, ++query_itr) + { + const PointT& point = *point_itr; + auto query = *query_itr; + point_representation_->vectorize (point, query); + } + + indices_mat.resize (points.size(), max_nn); + sqr_distances_mat.resize (points.size(), max_nn); + + ::flann::Matrix queries_mat_wrapper (queries_mat.data(), points.size(), dim_); + ::flann::Matrix sqr_distances_mat_wrapper (sqr_distances_mat.data(), points.size(), max_nn); + + int neighbors_in_radius = pcl::cuda::flann::radius_search(*flann_index_, + queries_mat_wrapper, + indices_mat, + sqr_distances_mat_wrapper, + static_cast(radius * radius), + params); + + // Do mapping to original point cloud + if (!identity_mapping_) + { + std::transform( + indices_mat.reshaped().cbegin(), + indices_mat.reshaped().cend(), + indices_mat.reshaped().begin(), + [this](const index_t& neighbor_index) + { return index_mapping_[neighbor_index]; } + ); + } + + return (neighbors_in_radius); +} + +template int +pcl::cuda::KdTreeFLANN::radiusSearch (const PointVector& points, + double radius, + IndicesVector& indices_vec, + DistancesVector& sqr_distances_vec, + unsigned int max_nn) const +{ + IndexMatrix indices_mat; + DistanceMatrix distances_mat; + + int neighbors_in_radius = radiusSearch(points, radius, indices_mat, distances_mat, max_nn); + + if (max_nn == 0 || max_nn > total_nr_points_) + max_nn = total_nr_points_; + + indices_vec.resize(points.size()); + sqr_distances_vec.resize(points.size()); + + // copy indices from matrix to vector + auto indices_row_itr = indices_mat.rowwise().cbegin(); + auto indices_itr = indices_vec.begin(); + + for (; indices_itr != indices_vec.end(); ++indices_row_itr, ++indices_itr) + { + auto indices_row = *indices_row_itr; + Indices& indices = *indices_itr; + + indices.clear(); + indices.reserve(max_nn); + + std::copy_if( + indices_row.cbegin(), + indices_row.cend(), + std::back_inserter(indices), + [](int index) + { return index != -1; } + ); + } + + // copy square distances from matrix to vector + auto distances_row_itr = distances_mat.rowwise().cbegin(); + auto distances_itr = sqr_distances_vec.begin(); + + for (; distances_itr != sqr_distances_vec.end(); ++distances_row_itr, ++distances_itr) + { + auto distances_row = *distances_row_itr; + std::vector& distances = *distances_itr; + + distances.clear(); + distances.reserve(max_nn); + + std::copy_if( + distances_row.cbegin(), + distances_row.cend(), + std::back_inserter(distances), + [](float distance) + { return std::isfinite(distance); } + ); + } + + return (neighbors_in_radius); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::cleanup () +{ + // Data array cleanup + index_mapping_.clear (); + + if (indices_) + indices_.reset (); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud) +{ + // No point in doing anything if the array is empty + if (cloud.empty ()) + { + cloud_.reset (); + return; + } + + const auto original_no_of_points = cloud.size (); + + cloud_.reset (new float[original_no_of_points * dim_], std::default_delete ()); + float* cloud_ptr = cloud_.get (); + index_mapping_.reserve (original_no_of_points); + identity_mapping_ = true; + + for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud[cloud_index])) + { + identity_mapping_ = false; + continue; + } + + index_mapping_.push_back (cloud_index); + + point_representation_->vectorize (cloud[cloud_index], cloud_ptr); + cloud_ptr += dim_; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::cuda::KdTreeFLANN::convertCloudToArray (const PointCloud &cloud, const Indices &indices) +{ + // No point in doing anything if the array is empty + if (cloud.empty ()) + { + cloud_.reset (); + return; + } + + int original_no_of_points = static_cast (indices.size ()); + + cloud_.reset (new float[original_no_of_points * dim_], std::default_delete ()); + float* cloud_ptr = cloud_.get (); + index_mapping_.reserve (original_no_of_points); + // its a subcloud -> false + // true only identity: + // - indices size equals cloud size + // - indices only contain values between 0 and cloud.size - 1 + // - no index is multiple times in the list + // => index is complete + // But we can not guarantee that => identity_mapping_ = false + identity_mapping_ = false; + + for (const auto &index : indices) + { + // Check if the point is invalid + if (!point_representation_->isValid (cloud[index])) + continue; + + // map from 0 - N -> indices [0] - indices [N] + index_mapping_.push_back (index); // If the returned index should be for the indices vector + + point_representation_->vectorize (cloud[index], cloud_ptr); + cloud_ptr += dim_; + } +} + +#define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::cuda::KdTreeFLANN; + +#endif //#ifndef PCL_CUDA_KDTREE_KDTREE_IMPL_FLANN_H_ + diff --git a/cuda/kdtree/include/pcl/cuda/kdtree/kdtree.h b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree.h new file mode 100644 index 00000000000..034b4f5d61f --- /dev/null +++ b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree.h @@ -0,0 +1,144 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * kdtree.h + * Adapted from: kdtree/include/pcl/kdtree/kdtree.h + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#pragma once + +#include + +namespace pcl { +namespace cuda { + /** \brief KdTree represents the base spatial locator class for kd-tree CUDA implementations. + * \author Ramzi Sabra + * \ingroup cuda/kdtree + */ + template + class KdTree : public KdTreeBase + { + using Base = KdTreeBase; + + public: + using PointVector = std::vector>; + using IndexMatrix = Eigen::Matrix; + using DistanceMatrix = Eigen::Matrix; + + using IndicesVector = std::vector; + using DistancesVector = std::vector>; + + // Boost shared pointers + using Ptr = shared_ptr >; + using ConstPtr = shared_ptr >; + + /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. + * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. Set to 64 by default. + */ + KdTree (bool sorted = true, int max_leaf_size = 64) + : Base(sorted, max_leaf_size) + { + }; + + /** \brief Search for k-nearest neighbors for the given query points. + * \param[in] points the given query points vector + * \param[in] k the number of neighbors to search for + * \param[out] k_indices_mat the resultant indices matrix of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances matrix to the neighboring points + * \return number of neighbors found + */ + virtual int + nearestKSearch (const PointVector& points, + unsigned int k, + IndexMatrix& k_indices_mat, + DistanceMatrix& k_sqr_distances_mat) const = 0; + + /** \brief Search for k-nearest neighbors for the given query points. + * \param[in] points the given query points + * \param[in] k the number of neighbors to search for + * \param[out] k_indices_mat the resultant indices vector of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances vector to the neighboring points + * \return number of neighbors found + */ + virtual int + nearestKSearch (const PointVector& points, + unsigned int k, + IndicesVector& k_indices_vec, + DistancesVector& k_sqr_distances_vec) const = 0; + + /** \brief Search for all the nearest neighbors of the query points in a given radius. + * \param[in] points the given query points vector + * \param[in] radius the radius of the spheres bounding all of each query point's neighbors + * \param[out] k_indices the resultant indices matrix of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances matrix to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + virtual int + radiusSearch (const PointVector& points, + double radius, + IndexMatrix& indices_mat, + DistanceMatrix& sqr_distances_mat, + unsigned int max_nn = 0) const = 0; + + /** \brief Search for all the nearest neighbors of the query points in a given radius. + * \param[in] points the given query points vector + * \param[in] radius the radius of the spheres bounding all of each query point's neighbors + * \param[out] k_indices the resultant indices vector of the neighboring points + * \param[out] k_sqr_distances the resultant squared distances vector to the neighboring points + * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to + * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be + * returned. + * \return number of neighbors found in radius + */ + virtual int + radiusSearch (const PointVector& points, + double radius, + IndicesVector& indices_vec, + DistancesVector& sqr_distances_vec, + unsigned int max_nn = 0) const = 0; + + protected: + using Base::input_; + using Base::indices_; + }; +} // namespace cuda +} // namespace pcl diff --git a/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h new file mode 100644 index 00000000000..f7696a2d3ac --- /dev/null +++ b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h @@ -0,0 +1,390 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * kdtree_flann.h + * Adapted from: kdtree/include/pcl/kdtree/kdtree_flann.h + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#pragma once + +#include +#include + +#include + +// Forward declarations +namespace flann +{ + template struct L2_Simple; + template class KDTreeCuda3dIndex; +} + +namespace pcl { +namespace cuda { +namespace flann { +namespace detail { +// Helper struct to create a compatible Matrix and copy data back when needed +// Replace using if constexpr in C++17 +template +struct compat_with_flann : std::false_type {}; + +template <> +struct compat_with_flann : std::true_type {}; + +template +using CompatWithFlann = std::enable_if_t::value, bool>; +template +using NotCompatWithFlann = std::enable_if_t::value, bool>; +} //namespace detail + +/** + * @brief Compatibility template function to allow use of various types of indices with + * FLANN + * @details Template is used for all params to not constrain any FLANN side capability + * @param[in,out] index A index searcher, of type ::flann::KDTreeCuda3dIndex or + * similar, where Dist is a template for computing distance between 2 points + * @param[in] queries A ::flann::Matrix or compatible matrix representation of the + * query point + * @param[out] indices Indices found in radius for each query point + * @param[out] dists Computed distance matrix + * @param[in] radius Threshold for consideration + * @param[in] params Any parameters to pass to the radius_search call + */ +template +int +radius_search(const FlannIndex& index, + const Queries& queries, + Indices& indices, + Distances& dists, + float radius, + const SearchParams& params); + +/** + * @brief Compatibility template function to allow use of various types of indices with + * FLANN + * @details Template is used for all params to not constrain any FLANN side capability + * @param[in,out] index A index searcher, of type ::flann::Index or similar, where + * Dist is a template for computing distance between 2 points + * @param[in] query A ::flann::Matrix or compatible matrix representation of the + * query point + * @param[out] indices Neighboring k indices found for each query point + * @param[out] dists Computed distance matrix + * @param[in] k Number of neighbors to search for + * @param[in] params Any parameters to pass to the knn_search call + */ +template +int +knn_search(const FlannIndex& index, + const Queries& queries, + Indices& indices, + Distances& dists, + unsigned int k, + const SearchParams& params); +} //namespace flann + +/** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures. + * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor) + * project by Marius Muja and David Lowe. + * + * \author Ramzi Sabra + * \ingroup cuda/kdtree + */ +template > +class KdTreeFLANN : public pcl::cuda::KdTree { + using Base = cuda::KdTree; + +protected: + using Base::input_; + using Base::indices_; + using Base::max_leaf_size_; + using Base::epsilon_; + using Base::sorted_; + using Base::point_representation_; + +public: + using PointCloud = typename Base::PointCloud; + using PointCloudConstPtr = typename Base::PointCloudConstPtr; + + using PointRepresentation = pcl::PointRepresentation; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; + + using IndicesPtr = shared_ptr; + using IndicesConstPtr = shared_ptr; + + using PointVector = typename Base::PointVector; + using IndexMatrix = typename Base::IndexMatrix; + using DistanceMatrix = typename Base::DistanceMatrix; + + using IndicesVector = typename Base::IndicesVector; + using DistancesVector = typename Base::DistancesVector; + + using FLANNIndex = ::flann::KDTreeCuda3dIndex; + + using Tree = KdTreeFLANN; + + // Shared pointers + using Ptr = shared_ptr; + using ConstPtr = shared_ptr; + + /** \brief Default Constructor for KdTreeFLANN. + * \param[in] sorted set to true if the application that the tree will be used for + * requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. Set to 64 by default. + * + * By setting sorted to false, the \ref radiusSearch operations will be faster. + */ + KdTreeFLANN(bool sorted = true, int max_leaf_size = 64); + + /** \brief Copy constructor + * \param[in] k the tree to copy into this + */ + KdTreeFLANN(const Tree& k); + + /** \brief Copy operator + * \param[in] k the tree to copy into this + */ + inline Tree& + operator=(const Tree& k) + { + cuda::KdTree::operator=(k); + flann_index_ = k.flann_index_; + cloud_ = k.cloud_; + index_mapping_ = k.index_mapping_; + identity_mapping_ = k.identity_mapping_; + total_nr_points_ = k.total_nr_points_; + param_k_ = k.param_k_; + param_radius_ = k.param_radius_; + return (*this); + } + + /** \brief Set the maximum leaf node size. + * \param[in] max_leaf_size maximum leaf node size + */ + void + setMaxLeafSize(int max_leaf_size) override; + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors + * searches. \param[in] eps precision (error bound) for nearest neighbors searches + */ + void + setEpsilon(float eps) override; + + void + setSortedResults(bool sorted); + + inline Ptr + makeShared() + { + return Ptr(new Tree(*this)); + } + + /** \brief Destructor for KdTreeFLANN. + * Deletes all allocated data arrays and destroys the kd-tree structures. + */ + ~KdTreeFLANN() { cleanup(); } + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud - if + * NULL the whole cloud is used + */ + void + setInputCloud(const PointCloudConstPtr& cloud, + const IndicesConstPtr& indices = IndicesConstPtr()) override; + + /** \brief Search for k-nearest neighbors for each of the given query points. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] points a given \a valid (i.e., finite) query points vector + * \param[in] k the number of neighbors to search for + * \param[out] k_indices_mat the resultant indices matrix of the neighboring points of each query + * point \param[out] k_sqr_distances_mat the resultant matrix of squared distances to the + * neighboring points of each query point \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + + /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. + * \param[in] point_representation the const shared pointer to a PointRepresentation + */ + inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) override; + + int + nearestKSearch(const PointVector& points, + unsigned int k, + IndexMatrix& k_indices_mat, + DistanceMatrix& k_sqr_distances_mat) const override; + + /** \brief Search for k-nearest neighbors for each of the given query points. + * + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] points a given \a valid (i.e., finite) query points vector + * \param[in] k the number of neighbors to search for + * \param[out] k_indices_vec the resultant indices vector of the neighboring points of each query + * point \param[out] k_sqr_distances_vec the resultant vector of squared distances to the + * neighboring points of each query point \return number of neighbors found + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + int + nearestKSearch(const PointVector& points, + unsigned int k, + IndicesVector& k_indices_vec, + DistancesVector& k_sqr_distances_vec) const override; + + /** \brief Search for all the nearest neighbors of each of the query points in a given + * radius. + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] points a given \a valid (i.e., finite) query points vector + * \param[in] radius the radius of the spheres bounding each query point's neighbors + * \param[out] indices_mat the resultant matrix of indices of the neighboring points of + * each query point \param[out] sqr_distances_mat the resultant matrix of squared + * distances to the neighboring points \param[in] max_nn if given, bounds the maximum + * returned neighbors for each query point to this value. If \a max_nn is set to 0 or + * to a number higher than the number of points in the input cloud, all neighbors in + * \a radius will be returned. \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + int + radiusSearch(const PointVector& points, + double radius, + IndexMatrix& indices_mat, + DistanceMatrix& sqr_distances_mat, + unsigned int max_nn = 0) const override; + + /** \brief Search for all the nearest neighbors of each of the query points in a given + * radius. + * \attention This method does not do any bounds checking for the input index + * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data. + * + * \param[in] points a given \a valid (i.e., finite) query points vector + * \param[in] radius the radius of the spheres bounding each query point's neighbors + * \param[out] indices_vec the resultant vector of indices of the neighboring points of + * each query point \param[out] sqr_distances_vec the resultant vector of squared + * distances to the neighboring points \param[in] max_nn if given, bounds the maximum + * returned neighbors for each query point to this value. If \a max_nn is set to 0 or + * to a number higher than the number of points in the input cloud, all neighbors in + * \a radius will be returned. \return number of neighbors found in radius + * + * \exception asserts in debug mode if the index is not between 0 and the maximum + * number of points + */ + int + radiusSearch(const PointVector& points, + double radius, + IndicesVector& indices_vec, + DistancesVector& sqr_distances_vec, + unsigned int max_nn = 0) const override; + +private: + /** \brief Internal cleanup method. */ + void + cleanup(); + + /** \brief Converts a PointCloud to the internal FLANN point array representation. + * Returns the number of points. \param cloud the PointCloud + */ + void + convertCloudToArray(const PointCloud& cloud); + + /** \brief Converts a PointCloud with a given set of indices to the internal FLANN + * point array representation. Returns the number of points. \param[in] cloud the + * PointCloud data \param[in] indices the point cloud indices + */ + void + convertCloudToArray(const PointCloud& cloud, const Indices& indices); + +private: + /** \brief Class getName method. */ + std::string + getName() const override + { + return ("cuda::KdTreeFLANN"); + } + + /** \brief A FLANN index object. */ + std::shared_ptr flann_index_; + + /** \brief Internal pointer to data. TODO: replace with std::shared_ptr with + * C++17*/ + std::shared_ptr cloud_; + + /** \brief mapping between internal and external indices. */ + std::vector index_mapping_; + + /** \brief whether the mapping between internal and external indices is identity */ + bool identity_mapping_; + + /** \brief Tree dimensionality (i.e. the number of dimensions per point) - fixed to 3. */ + constexpr static int dim_ = 3; + + /** \brief The total size of the data (either equal to the number of points in the + * input cloud or to the number of indices - if passed). */ + uindex_t total_nr_points_; + + /** \brief The KdTree search parameters for K-nearest neighbors. */ + ::flann::SearchParams param_k_; + + /** \brief The KdTree search parameters for radius search. */ + ::flann::SearchParams param_radius_; + }; +} //namespace cuda +} //namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/cuda/kdtree/src/kdtree_flann.cpp b/cuda/kdtree/src/kdtree_flann.cpp new file mode 100644 index 00000000000..d88df68943c --- /dev/null +++ b/cuda/kdtree/src/kdtree_flann.cpp @@ -0,0 +1,47 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +PCL_INSTANTIATE(KdTreeFLANN, PCL_POINT_TYPES) +#endif // PCL_NO_PRECOMPILE + diff --git a/kdtree/CMakeLists.txt b/kdtree/CMakeLists.txt index cd9804b57d4..d1ef5152962 100644 --- a/kdtree/CMakeLists.txt +++ b/kdtree/CMakeLists.txt @@ -28,6 +28,7 @@ set(srcs ) set(incs + "include/pcl/${SUBSYS_NAME}/base.h" "include/pcl/${SUBSYS_NAME}/kdtree.h" "include/pcl/${SUBSYS_NAME}/io.h" "include/pcl/${SUBSYS_NAME}/kdtree_flann.h" diff --git a/kdtree/include/pcl/kdtree/base.h b/kdtree/include/pcl/kdtree/base.h new file mode 100644 index 00000000000..6eda04631ad --- /dev/null +++ b/kdtree/include/pcl/kdtree/base.h @@ -0,0 +1,196 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace pcl +{ + template + class KdTreeBase + { + public: + using IndicesPtr = shared_ptr; + using IndicesConstPtr = shared_ptr; + + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + + using PointRepresentation = pcl::PointRepresentation; + using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; + + /** \brief Empty constructor for KdTreeBase. Sets some internal values to their defaults. + * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. + * \param[in] max_leaf_size maximum leaf node size. + */ + KdTreeBase (bool sorted, int max_leaf_size) + : input_() + , max_leaf_size_(max_leaf_size) + , epsilon_(0.0f) + , min_pts_(1) + , sorted_(sorted) + , point_representation_ (new DefaultPointRepresentation) + { + } + + /** \brief Destructor for KdTreeBase. Deletes all allocated data arrays and destroys the kd-tree structures. */ + virtual ~KdTreeBase () {}; + + /** \brief Provide a pointer to the input dataset. + * \param[in] cloud the const shared pointer to a PointCloud message + * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used + */ + virtual void + setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) + { + input_ = cloud; + indices_ = indices; + } + + /** \brief Get a pointer to the input point cloud dataset. */ + inline PointCloudConstPtr + getInputCloud () const + { + return (input_); + } + + /** \brief Get a pointer to the vector of indices used. */ + inline IndicesConstPtr + getIndices () const + { + return (indices_); + } + + /** \brief Set the maximum leaf node size. + * \param[in] max_leaf_size maximum leaf node size + */ + virtual inline void + setMaxLeafSize(int max_leaf_size) + { + max_leaf_size_ = max_leaf_size; + } + + /** \brief Get the maximum leaf node size. */ + inline int getMaxLeafSize() const + { + return (max_leaf_size_); + } + + /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. + * \param[in] eps precision (error bound) for nearest neighbors searches + */ + virtual inline void + setEpsilon (float eps) + { + epsilon_ = eps; + } + + /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ + inline float + getEpsilon () const + { + return (epsilon_); + } + + /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. + * \param[in] min_pts the minimum number of neighbors in a viable neighborhood + */ + inline void + setMinPts (int min_pts) + { + min_pts_ = min_pts; + } + + /** \brief Get the minimum allowed number of k nearest neighbors points that a viable result must contain. */ + inline int + getMinPts () const + { + return (min_pts_); + } + + /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. + * \param[in] point_representation the const shared pointer to a PointRepresentation + */ + virtual inline void + setPointRepresentation (const PointRepresentationConstPtr &point_representation) + { + point_representation_ = point_representation; + if (!input_) return; + setInputCloud (input_, indices_); // Makes sense in derived classes to reinitialize the tree + } + + /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ + inline PointRepresentationConstPtr + getPointRepresentation () const + { + return (point_representation_); + } + + protected: + /** \brief The input point cloud dataset containing the points we need to use. */ + PointCloudConstPtr input_; + + /** \brief A pointer to the vector of point indices to use. */ + IndicesConstPtr indices_; + + /** \brief Maximum leaf node size */ + int max_leaf_size_; + + /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ + float epsilon_; + + /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. */ + int min_pts_; + + /** \brief Return the radius search neighbours sorted **/ + bool sorted_; + + /** \brief For converting different point structures into k-dimensional vectors for nearest-neighbor search. */ + PointRepresentationConstPtr point_representation_; + + /** \brief Class getName method. */ + virtual std::string + getName () const = 0; + }; +} diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h index 8c4cb3626ba..39c21684576 100644 --- a/kdtree/include/pcl/kdtree/kdtree.h +++ b/kdtree/include/pcl/kdtree/kdtree.h @@ -38,11 +38,7 @@ #pragma once -#include -#include -#include -#include -#include +#include namespace pcl { @@ -51,79 +47,28 @@ namespace pcl * \ingroup kdtree */ template - class KdTree + class KdTree : public KdTreeBase { - public: - using IndicesPtr = shared_ptr; - using IndicesConstPtr = shared_ptr; + using Base = KdTreeBase; + public: using PointCloud = pcl::PointCloud; - using PointCloudPtr = typename PointCloud::Ptr; - using PointCloudConstPtr = typename PointCloud::ConstPtr; - - using PointRepresentation = pcl::PointRepresentation; - using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; - // Boost shared pointers + // Shared pointers using Ptr = shared_ptr >; using ConstPtr = shared_ptr >; + using Base::setInputCloud; + /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. * \param[in] max_leaf_size maximum leaf node size. Set to 15 by default. */ - KdTree (bool sorted = true, int max_leaf_size = 15) : input_(), - max_leaf_size_(max_leaf_size), epsilon_(0.0f), min_pts_(1), sorted_(sorted), - point_representation_ (new DefaultPointRepresentation) + KdTree (bool sorted = true, int max_leaf_size = 15) + : Base (sorted, max_leaf_size) { }; - /** \brief Provide a pointer to the input dataset. - * \param[in] cloud the const boost shared pointer to a PointCloud message - * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used - */ - virtual void - setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) - { - input_ = cloud; - indices_ = indices; - } - - /** \brief Get a pointer to the vector of indices used. */ - inline IndicesConstPtr - getIndices () const - { - return (indices_); - } - - /** \brief Get a pointer to the input point cloud dataset. */ - inline PointCloudConstPtr - getInputCloud () const - { - return (input_); - } - - /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. - * \param[in] point_representation the const boost shared pointer to a PointRepresentation - */ - inline void - setPointRepresentation (const PointRepresentationConstPtr &point_representation) - { - point_representation_ = point_representation; - if (!input_) return; - setInputCloud (input_, indices_); // Makes sense in derived classes to reinitialize the tree - } - - /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ - inline PointRepresentationConstPtr - getPointRepresentation () const - { - return (point_representation_); - } - - /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */ - virtual ~KdTree () {}; - /** \brief Search for k-nearest neighbors for the given query point. * \param[in] p_q the given query point * \param[in] k the number of neighbors to search for @@ -300,77 +245,8 @@ namespace pcl return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); } - /** \brief Set the maximum leaf node size. - * \param[in] max_leaf_size maximum leaf node size - */ - virtual inline void - setMaxLeafSize(int max_leaf_size) - { - max_leaf_size_ = max_leaf_size; - } - - /** \brief Get the maximum leaf node size. */ - inline int getMaxLeafSize() const - { - return (max_leaf_size_); - } - - /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. - * \param[in] eps precision (error bound) for nearest neighbors searches - */ - virtual inline void - setEpsilon (float eps) - { - epsilon_ = eps; - } - - /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ - inline float - getEpsilon () const - { - return (epsilon_); - } - - /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. - * \param[in] min_pts the minimum number of neighbors in a viable neighborhood - */ - inline void - setMinPts (int min_pts) - { - min_pts_ = min_pts; - } - - /** \brief Get the minimum allowed number of k nearest neighbors points that a viable result must contain. */ - inline int - getMinPts () const - { - return (min_pts_); - } - protected: - /** \brief The input point cloud dataset containing the points we need to use. */ - PointCloudConstPtr input_; - - /** \brief A pointer to the vector of point indices to use. */ - IndicesConstPtr indices_; - - /** \brief Maximum leaf node size */ - int max_leaf_size_; - - /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ - float epsilon_; - - /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. */ - int min_pts_; - - /** \brief Return the radius search neighbours sorted **/ - bool sorted_; - - /** \brief For converting different point structures into k-dimensional vectors for nearest-neighbor search. */ - PointRepresentationConstPtr point_representation_; - - /** \brief Class getName method. */ - virtual std::string - getName () const = 0; + using Base::input_; + using Base::indices_; }; -} +} // namespace pcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cb7f77530de..1a9c04b28a9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,7 @@ include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") add_subdirectory(2d) add_subdirectory(common) +add_subdirectory(cuda) add_subdirectory(features) add_subdirectory(filters) add_subdirectory(geometry) diff --git a/test/cuda/CMakeLists.txt b/test/cuda/CMakeLists.txt new file mode 100644 index 00000000000..afb7e8cc384 --- /dev/null +++ b/test/cuda/CMakeLists.txt @@ -0,0 +1,5 @@ +if(NOT HAVE_CUDA) + return() +endif() + +add_subdirectory(kdtree) diff --git a/test/cuda/kdtree/CMakeLists.txt b/test/cuda/kdtree/CMakeLists.txt new file mode 100644 index 00000000000..60a26f2bbc1 --- /dev/null +++ b/test/cuda/kdtree/CMakeLists.txt @@ -0,0 +1,19 @@ +set(SUBSYS_NAME tests_cuda_kdtree) +set(SUBSYS_DESC "Point cloud library kd-tree CUDA tests") +set(SUBSYS_DEPS common kdtree cuda_kdtree) +set(OPT_DEPS io) # io is not a mandatory dependency in kdtree + +set(DEFAULT ON) +set(build TRUE) +set(REASON "") +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${OPT_DEPS}) + +if(NOT (build AND BUILD_io)) + return() +endif() + +PCL_ADD_TEST (cuda_kdtree test_cuda_kdtree + FILES test_kdtree.cpp + LINK_WITH pcl_gtest pcl_io pcl_common pcl_kdtree pcl_cuda_kdtree + ARGUMENTS "${PCL_SOURCE_DIR}/test/sac_plane_test.pcd" "${PCL_SOURCE_DIR}/test/kdtree/kdtree_unit_test_results.xml") diff --git a/test/cuda/kdtree/test_kdtree.cpp b/test/cuda/kdtree/test_kdtree.cpp new file mode 100644 index 00000000000..0364124796f --- /dev/null +++ b/test/cuda/kdtree/test_kdtree.cpp @@ -0,0 +1,664 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * test_kdtree.h + * Adapted from: test/kdtree/test_kdtree.cpp + * Created on: Jun 01, 2022 + * Author: Ramzi Sabra + */ + +#include + +#include +#include +#include +#include // for pcl::isFinite +#include +#include +#include + +#include +#include + +#include +#include // For debug +#include +#include + +using namespace pcl; + +boost::property_tree::ptree xml_property_tree; + + +PointCloud::Ptr cloud_in (new PointCloud ()); + +struct MyPoint : public PointXYZ +{ + MyPoint (float x, float y, float z) {this->x=x; this->y=y; this->z=z;} +}; + +using PointVector = std::vector>; + +using IndexMatrix = Eigen::Matrix; +using DistanceMatrix = Eigen::Matrix; + +template +class PCLCUDAKdTreeTestFixture : public ::testing::Test +{ + public: + using TreeMyPoint = std::tuple_element_t<0, TupleType>; + using TreePointXY = std::tuple_element_t<1, TupleType>; + + PointCloud cloud_, cloud_big_; + + PCLCUDAKdTreeTestFixture() + { + float resolution = 0.1f; + for (float z = -0.5f; z <= 0.5f; z += resolution) + for (float y = -0.5f; y <= 0.5f; y += resolution) + for (float x = -0.5f; x <= 0.5f; x += resolution) + cloud_.emplace_back(x, y, z); + cloud_.width = cloud_.size (); + cloud_.height = 1; + + cloud_big_.width = 640; + cloud_big_.height = 480; + srand (static_cast (time (nullptr))); + // Randomly create a new point cloud + for (std::size_t i = 0; i < cloud_big_.width * cloud_big_.height; ++i) + cloud_big_.emplace_back(static_cast (1024 * rand () / (RAND_MAX + 1.0)), + static_cast (1024 * rand () / (RAND_MAX + 1.0)), + static_cast (1024 * rand () / (RAND_MAX + 1.0))); + } +}; + +using KdTreeTestTypes = ::testing::Types< + std::tuple, cuda::KdTreeFLANN> +>; +TYPED_TEST_SUITE(PCLCUDAKdTreeTestFixture, KdTreeTestTypes); + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TYPED_TEST (PCLCUDAKdTreeTestFixture, KdTree_radiusSearch) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + auto& cloud_big = this->cloud_big_; + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + MyPoint test_point(0.0f, 0.0f, 0.0f); + unsigned int num_points = 4; + double max_dist = 0.15; + int max_nn = 100; + std::set brute_force_result; + for (std::size_t i=0; i < cloud.size(); ++i) + if (euclideanDistance(cloud[i], test_point) < max_dist) + brute_force_result.insert(i); + + PointVector test_points; + test_points.reserve(num_points); + for (int i = 0; i < num_points; ++i) + test_points.push_back(test_point); + + IndexMatrix indices_mat; + DistanceMatrix distances_mat; + kdtree.radiusSearch (test_points, max_dist, indices_mat, distances_mat, max_nn); + + std::vector indices_vec; + std::vector> distances_vec; + kdtree.radiusSearch (test_points, max_dist, indices_vec, distances_vec, max_nn); + + //std::cout << k_indices.size()<<"=="< indices_row_vec; + indices_row_vec.reserve(max_nn); + + // remove -1 for cases where (#NN for a point) < k (due to fixed number of columns = k in matrix) + std::copy_if( + indices_row.cbegin(), + indices_row.cend(), + std::back_inserter(indices_row_vec), + [](const index_t& index) + { return index != -1; } + ); + + EXPECT_EQ(indices_row_vec, indices); + } + + // check if distances are equal across matrix and vector representations + auto distances_row_itr = distances_mat.rowwise().cbegin(); + auto distances_itr = distances_vec.cbegin(); + + for (; distances_itr != distances_vec.cend(); ++distances_row_itr, ++distances_itr) + { + auto distances_row = *distances_row_itr; + const auto& distances = *distances_itr; + + EXPECT_LE(distances.size(), max_nn); + + std::vector distances_row_vec; + distances_row_vec.reserve(max_nn); + + // remove infinite distances for cases where (#NN for a point) < k (due to fixed number of columns = k in matrix) + std::copy_if( + distances_row.cbegin(), + distances_row.cend(), + std::back_inserter(distances_row_vec), + [](const float& distance) + { return std::isfinite(distance); } + ); + + EXPECT_EQ(distances_row_vec, distances); + } + + for (const auto& indices : indices_vec) + { + auto brute_force_result_copy = brute_force_result; + for (const auto &index : indices) + { + std::set::iterator brute_force_result_it = brute_force_result_copy.find (index); + bool ok = brute_force_result_it != brute_force_result_copy.end (); + //if (!ok) std::cerr << index << " is not correct...\n"; + //else std::cerr << index << " is correct...\n"; + EXPECT_TRUE (ok); + if (ok) + brute_force_result_copy.erase (brute_force_result_it); + } + for (std::set::const_iterator it=brute_force_result_copy.cbegin(); it!=brute_force_result_copy.cend(); ++it) + std::cerr << "FLANN missed "<<*it<<"\n"; + + bool error = !brute_force_result_copy.empty (); + if (error) std::cerr << "Missed too many neighbors!\n"; + EXPECT_FALSE (error); + } + + { + Tree kdtree; + kdtree.setInputCloud (cloud_big.makeShared ()); + + ScopeTime scopeTime ("FLANN CUDA radiusSearch"); + { + kdtree.radiusSearch (cloud_big.points, 0.1, indices_vec, distances_vec); + } + } + + { + Tree kdtree; + kdtree.setInputCloud (cloud_big.makeShared ()); + + ScopeTime scopeTime ("FLANN CUDA radiusSearch (max neighbors in radius)"); + { + kdtree.radiusSearch (cloud_big.points, 0.1, indices_vec, distances_vec, 10); + } + } + + + { + Tree kdtree (false); + kdtree.setInputCloud (cloud_big.makeShared ()); + + ScopeTime scopeTime ("FLANN CUDA radiusSearch (unsorted results)"); + { + kdtree.radiusSearch (cloud_big.points, 0.1, indices_vec, distances_vec); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TYPED_TEST (PCLCUDAKdTreeTestFixture, KdTree_nearestKSearch) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + auto& cloud_big = this->cloud_big_; + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + MyPoint test_point (0.01f, 0.01f, 0.01f); + unsigned int num_points = 4; + unsigned int no_of_neighbors = 20; + std::multimap sorted_brute_force_result; + for (std::size_t i = 0; i < cloud.size (); ++i) + { + float distance = euclideanDistance (cloud[i], test_point); + sorted_brute_force_result.insert (std::make_pair (distance, static_cast (i))); + } + float max_dist = 0.0f; + unsigned int counter = 0; + for (std::multimap::iterator it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end () && counter < no_of_neighbors; ++it) + { + max_dist = std::max (max_dist, it->first); + ++counter; + } + + PointVector test_points; + test_points.reserve(num_points); + for (int i = 0; i < num_points; ++i) + test_points.push_back(test_point); + + IndexMatrix k_indices_mat; + DistanceMatrix k_distances_mat; + kdtree.nearestKSearch (test_points, no_of_neighbors, k_indices_mat, k_distances_mat); + + std::vector k_indices_vec; + std::vector> k_distances_vec; + kdtree.nearestKSearch (test_points, no_of_neighbors, k_indices_vec, k_distances_vec); + + EXPECT_EQ(k_indices_mat.rows(), num_points); + EXPECT_EQ(k_indices_mat.cols(), no_of_neighbors); + EXPECT_EQ(k_distances_mat.rows(), num_points); + EXPECT_EQ(k_distances_mat.cols(), no_of_neighbors); + + EXPECT_EQ(k_indices_vec.size(), num_points); + EXPECT_EQ(k_distances_vec.size(), num_points); + + // check if indices are equal across matrix and vector representations + auto k_indices_row_itr = k_indices_mat.rowwise().cbegin(); + auto k_indices_itr = k_indices_vec.cbegin(); + + for (; k_indices_itr != k_indices_vec.cend(); ++k_indices_row_itr, ++k_indices_itr) + { + auto k_indices_row = *k_indices_row_itr; + const auto& k_indices = *k_indices_itr; + + EXPECT_EQ(k_indices.size(), no_of_neighbors); + + std::vector k_indices_row_vec (no_of_neighbors); + + std::copy( + k_indices_row.cbegin(), + k_indices_row.cend(), + k_indices_row_vec.begin() + ); + + EXPECT_EQ(k_indices_row_vec, k_indices); + } + + // check if distances are equal across matrix and vector representations + auto k_distances_row_itr = k_distances_mat.rowwise().cbegin(); + auto k_distances_itr = k_distances_vec.cbegin(); + + for (; k_distances_itr != k_distances_vec.cend(); ++k_distances_row_itr, ++k_distances_itr) + { + auto k_distances_row = *k_distances_row_itr; + const auto& k_distances = *k_distances_itr; + + EXPECT_EQ(k_distances.size(), no_of_neighbors); + + std::vector k_distances_row_vec (no_of_neighbors); + + std::copy( + k_distances_row.cbegin(), + k_distances_row.cend(), + k_distances_row_vec.begin() + ); + + EXPECT_EQ(k_distances_row_vec, k_distances); + } + + for (const auto& k_indices : k_indices_vec) + { + EXPECT_EQ (k_indices.size (), no_of_neighbors); + + // Check if all found neighbors have distance smaller than max_dist + for (const auto &k_index : k_indices) + { + const MyPoint& point = cloud[k_index]; + bool ok = euclideanDistance (test_point, point) <= max_dist; + if (!ok) + ok = (std::abs (euclideanDistance (test_point, point)) - max_dist) <= 1e-6; + //if (!ok) std::cerr << k_index << " is not correct...\n"; + //else std::cerr << k_index << " is correct...\n"; + EXPECT_TRUE (ok); + } + } + + ScopeTime scopeTime ("FLANN CUDA nearestKSearch"); + { + Tree kdtree; + kdtree.setInputCloud (cloud_big.makeShared ()); + kdtree.nearestKSearch (cloud_big.points, no_of_neighbors, k_indices_vec, k_distances_vec); + } +} + +class MyPointRepresentationXYZ : public PointRepresentation +{ + public: + MyPointRepresentationXYZ () + { + this->nr_dimensions_ = 3; + } + + void copyToFloatArray (const MyPoint &p, float *out) const override + { + out[0] = p.x; + out[1] = p.y; + out[2] = p.z; + } +}; + +class MyPointRepresentationXY : public PointRepresentation +{ + public: + MyPointRepresentationXY () + { + this->nr_dimensions_ = 2; + } + + void copyToFloatArray (const MyPoint &p, float *out) const override + { + out[0] = p.x; + out[1] = p.y; + } +}; + +TYPED_TEST (PCLCUDAKdTreeTestFixture, KdTree_setPointRepresentation) +{ + using Tree = typename TestFixture::TreeMyPoint; + using TreePointXY = typename TestFixture::TreePointXY; + + PointCloud::Ptr random_cloud (new PointCloud ()); + random_cloud->points.emplace_back(86.6f, 42.1f, 92.4f); + random_cloud->points.emplace_back(63.1f, 18.4f, 22.3f); + random_cloud->points.emplace_back(35.5f, 72.5f, 37.3f); + random_cloud->points.emplace_back(99.7f, 37.0f, 8.7f); + random_cloud->points.emplace_back(22.4f, 84.1f, 64.0f); + random_cloud->points.emplace_back(65.2f, 73.4f, 18.0f); + random_cloud->points.emplace_back(60.4f, 57.1f, 4.5f); + random_cloud->points.emplace_back(38.7f, 17.6f, 72.3f); + random_cloud->points.emplace_back(14.2f, 95.7f, 34.7f); + random_cloud->points.emplace_back( 2.5f, 26.5f, 66.0f); + + Tree kdtree; + kdtree.setInputCloud (random_cloud); + MyPoint test_point (50.0f, 50.0f, 50.0f); + unsigned int num_points = 4; + + PointVector test_points; + test_points.reserve(num_points); + for (int i = 0; i < num_points; ++i) + test_points.push_back(test_point); + + const int k = 10; + std::vector k_indices_vec(num_points); + std::vector> k_distances_vec(num_points); + + // Find k nearest neighbors + { + kdtree.nearestKSearch (test_points, k, k_indices_vec, k_distances_vec); + + auto k_indices_itr = k_indices_vec.cbegin(); + auto k_distances_itr = k_distances_vec.cbegin(); + + for (; k_indices_itr != k_indices_vec.cend(); ++k_indices_itr, ++k_distances_itr) + { + auto& k_indices = *k_indices_itr; + auto& k_distances = *k_distances_itr; + + for (int i = 0; i < k; ++i) + { + // Compare to ground truth values, computed independently + static const int gt_indices[10] = {2, 7, 5, 1, 4, 6, 9, 0, 8, 3}; + static const float gt_distances[10] = + {877.8f, 1674.7f, 1802.6f, 1937.5f, 2120.6f, 2228.8f, 3064.5f, 3199.7f, 3604.2f, 4344.8f}; + EXPECT_EQ (k_indices[i], gt_indices[i]); + EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1); + } + } + } + + // Find k nearest neighbors with a different point representation + { + k_indices_vec.clear(); + k_distances_vec.clear(); + + typename Tree::PointRepresentationConstPtr ptrep (new MyPointRepresentationXYZ); + kdtree.setPointRepresentation (ptrep); + kdtree.nearestKSearch (test_points, k, k_indices_vec, k_distances_vec); + + auto k_indices_itr = k_indices_vec.cbegin(); + auto k_distances_itr = k_distances_vec.cbegin(); + + for (; k_indices_itr != k_indices_vec.cend(); ++k_indices_itr, ++k_distances_itr) + { + auto& k_indices = *k_indices_itr; + auto& k_distances = *k_distances_itr; + + for (int i = 0; i < k; ++i) + { + // Compare to ground truth values, computed independently + static const int gt_indices[10] = {2, 7, 5, 1, 4, 6, 9, 0, 8, 3}; + static const float gt_distances[10] = + {877.8f, 1674.7f, 1802.6f, 1937.5f, 2120.6f, 2228.8f, 3064.5f, 3199.7f, 3604.2f, 4344.8f}; + EXPECT_EQ (k_indices[i], gt_indices[i]); + EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1); + } + } + } + + // Go back to the default, this time with the values rescaled + { + DefaultPointRepresentation point_rep; + float alpha[3] = {1.0f, 2.0f, 3.0f}; + point_rep.setRescaleValues(alpha); + kdtree.setPointRepresentation (point_rep.makeShared ()); + kdtree.nearestKSearch (test_points, k, k_indices_vec, k_distances_vec); + + auto k_indices_itr = k_indices_vec.cbegin(); + auto k_distances_itr = k_distances_vec.cbegin(); + + for (; k_indices_itr != k_indices_vec.cend(); ++k_indices_itr, ++k_distances_itr) + { + auto& k_indices = *k_indices_itr; + auto& k_distances = *k_distances_itr; + + for (int i = 0; i < k; ++i) + { + // Compare to ground truth values, computed independently + static const int gt_indices[10] = {2, 9, 4, 7, 1, 5, 8, 0, 3, 6}; + static const float gt_distances[10] = + {3686.9f, 6769.2f, 7177.0f, 8802.3f, 11071.5f, 11637.3f, 11742.4f, 17769.0f, 18497.3f, 18942.0f}; + EXPECT_EQ (k_indices[i], gt_indices[i]); + EXPECT_NEAR (k_distances[i], gt_distances[i], 0.1); + } + } + } + + // Construct a kd-tree for points with 2 dimensions + EXPECT_THROW (TreePointXY(), std::domain_error); + + // Set a point representation with 2 dimensions + { + typename Tree::PointRepresentationConstPtr ptrep (new MyPointRepresentationXY); + EXPECT_THROW(kdtree.setPointRepresentation(ptrep), std::domain_error); + } +} + +TYPED_TEST (PCLCUDAKdTreeTestFixture, KdTree_copy) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + + MyPoint test_point (0.01f, 0.01f, 0.01f); + const int k = 10; + const int max_nn = 10; + unsigned int num_points = 4; + + PointVector test_points; + test_points.reserve(num_points); + for (int i = 0; i < num_points; ++i) + test_points.push_back(test_point); + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + + std::vector k_indices_vec; + std::vector> k_distances_vec; + kdtree.nearestKSearch (test_points, k, k_indices_vec, k_distances_vec); + + std::vector indices_vec; + std::vector> distances_vec; + kdtree.radiusSearch (test_points, max_nn, indices_vec, distances_vec); + + Tree kdtree_copy (kdtree); + + std::vector k_indices_vec_copy; + std::vector> k_distances_vec_copy; + kdtree_copy.nearestKSearch (test_points, k, k_indices_vec_copy, k_distances_vec_copy); + + std::vector indices_vec_copy; + std::vector> distances_vec_copy; + kdtree_copy.radiusSearch (test_points, max_nn, indices_vec_copy, distances_vec_copy); + + EXPECT_EQ (k_indices_vec, k_indices_vec_copy); + EXPECT_EQ (k_distances_vec, k_distances_vec_copy); + EXPECT_EQ (indices_vec, indices_vec_copy); + EXPECT_EQ (distances_vec, distances_vec_copy); +} + +TYPED_TEST (PCLCUDAKdTreeTestFixture, KdTree_setMaxLeafSize) +{ + using Tree = typename TestFixture::TreeMyPoint; + + auto& cloud = this->cloud_; + + MyPoint test_point (0.01f, 0.01f, 0.01f); + const int k = 10; + unsigned int num_points = 4; + + PointVector test_points; + test_points.reserve(num_points); + for (int i = 0; i < num_points; ++i) + test_points.push_back(test_point); + + Tree kdtree; + kdtree.setInputCloud (cloud.makeShared ()); + + std::vector k_indices_default_vec; + std::vector> k_distances_default_vec; + kdtree.nearestKSearch (test_points, k, k_indices_default_vec, k_distances_default_vec); + + kdtree.setMaxLeafSize (50); + + std::vector k_indices_changed_vec; + std::vector> k_distances_changed_vec; + kdtree.nearestKSearch (test_points, k, k_indices_changed_vec, k_distances_changed_vec); + + auto k_indices_default_itr = k_indices_default_vec.cbegin(); + auto k_indices_changed_itr = k_indices_changed_vec.cbegin(); + + for (; k_indices_default_itr != k_indices_default_vec.cend(); ++k_indices_default_itr, ++k_indices_changed_itr) + { + const pcl::Indices& k_indices_default = *k_indices_default_itr; + const pcl::Indices& k_indices_changed = *k_indices_changed_itr; + + std::unordered_set k_indices_set_default, k_indices_set_changed; + + std::copy( + k_indices_default.cbegin(), + k_indices_default.cend(), + std::inserter( + k_indices_set_default, + k_indices_set_default.end() + ) + ); + + std::copy( + k_indices_changed.cbegin(), + k_indices_changed.cend(), + std::inserter( + k_indices_set_changed, + k_indices_set_changed.end() + ) + ); + + EXPECT_EQ (k_indices_set_default, k_indices_set_changed); + } + + auto k_distances_default_itr = k_distances_default_vec.cbegin(); + auto k_distances_changed_itr = k_distances_changed_vec.cbegin(); + + for (; k_distances_default_itr != k_distances_default_vec.cend(); ++k_distances_default_itr, ++k_distances_changed_itr) + { + const std::vector& k_distances_default = *k_distances_default_itr; + const std::vector& k_distances_changed = *k_distances_changed_itr; + + EXPECT_EQ (k_distances_default, k_distances_changed); + } +} + +/* ---[ */ +int +main (int argc, char** argv) +{ + // Load the standard PCD file from disk + if (argc < 3) + { + std::cerr << "No test file given. Please download `sac_plane_test.pcd` and 'kdtree_unit_test_results.xml' pass them path to the test." << std::endl; + return (-1); + } + + // Load in the point clouds + io::loadPCDFile (argv[1], *cloud_in); + + std::ifstream xml_file_input_stream (argv[2], std::ifstream::in); + read_xml (xml_file_input_stream, xml_property_tree, boost::property_tree::xml_parser::trim_whitespace); + + testing::InitGoogleTest (&argc, argv); + + return (RUN_ALL_TESTS ()); +} +/* ]--- */ From fefead340254cb6f10631e3f61ca2c88312d0939 Mon Sep 17 00:00:00 2001 From: Ramzi Sabra Date: Mon, 6 Jun 2022 20:06:12 +0300 Subject: [PATCH 5/5] fixed KdTree documentation --- cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h | 14 +++++++------- kdtree/include/pcl/kdtree/kdtree_nanoflann.h | 2 +- kdtree/kdtree.doxy | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h index f7696a2d3ac..c5107df6250 100644 --- a/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h +++ b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h @@ -145,8 +145,8 @@ class KdTreeFLANN : public pcl::cuda::KdTree { using Base::point_representation_; public: - using PointCloud = typename Base::PointCloud; - using PointCloudConstPtr = typename Base::PointCloudConstPtr; + using PointCloud = pcl::PointCloud; + using PointCloudConstPtr = typename PointCloud::ConstPtr; using PointRepresentation = pcl::PointRepresentation; using PointRepresentationConstPtr = typename PointRepresentation::ConstPtr; @@ -154,12 +154,12 @@ class KdTreeFLANN : public pcl::cuda::KdTree { using IndicesPtr = shared_ptr; using IndicesConstPtr = shared_ptr; - using PointVector = typename Base::PointVector; - using IndexMatrix = typename Base::IndexMatrix; - using DistanceMatrix = typename Base::DistanceMatrix; + using PointVector = typename cuda::KdTree::PointVector; + using IndexMatrix = typename cuda::KdTree::IndexMatrix; + using DistanceMatrix = typename cuda::KdTree::DistanceMatrix; - using IndicesVector = typename Base::IndicesVector; - using DistancesVector = typename Base::DistancesVector; + using IndicesVector = typename cuda::KdTree::IndicesVector; + using DistancesVector = typename cuda::KdTree::DistancesVector; using FLANNIndex = ::flann::KDTreeCuda3dIndex; diff --git a/kdtree/include/pcl/kdtree/kdtree_nanoflann.h b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h index ef9d74b1562..75a30e3ae07 100644 --- a/kdtree/include/pcl/kdtree/kdtree_nanoflann.h +++ b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h @@ -192,7 +192,7 @@ class KdTreeNanoflann : public pcl::KdTree { return Ptr(new Tree(*this)); } - /** \brief Destructor for KdTreeFLANN. + /** \brief Destructor for KdTreeNanoflann. * Deletes all allocated data arrays and destroys the kd-tree structures. */ ~KdTreeNanoflann() { cleanup(); } diff --git a/kdtree/kdtree.doxy b/kdtree/kdtree.doxy index 9bf527c3206..1f446b05d6c 100644 --- a/kdtree/kdtree.doxy +++ b/kdtree/kdtree.doxy @@ -4,7 +4,7 @@ \section secKDtreePresentation Overview The pcl_kdtree library provides the kd-tree data-structure, using - FLANN, + FLANN / nanoflann, that allows for fast nearest neighbor searches. A Kd-tree (k-dimensional tree) is a space-partitioning data