diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fbfaf15f1f..396066c5f4d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -324,6 +324,9 @@ if(NOT (${FLANN_LIBRARY_TYPE} MATCHES ${PCL_FLANN_REQUIRED_TYPE}) AND NOT (${PCL message(FATAL_ERROR "Flann was selected with ${PCL_FLANN_REQUIRED_TYPE} but found as ${FLANN_LIBRARY_TYPE}") endif() +# nanoflann (required) +find_package(nanoflann 1.4.2 REQUIRED) + # libusb option(WITH_LIBUSB "Build USB RGBD-Camera drivers" TRUE) if(WITH_LIBUSB) diff --git a/cuda/kdtree/CMakeLists.txt b/cuda/kdtree/CMakeLists.txt new file mode 100644 index 00000000000..c9ad55f8954 --- /dev/null +++ b/cuda/kdtree/CMakeLists.txt @@ -0,0 +1,41 @@ +set(SUBSYS_NAME cuda_kdtree) +set(SUBSYS_PATH cuda/kdtree) +set(SUBSYS_DESC "Point cloud CUDA kd-tree library") +set(SUBSYS_DEPS cuda_common io common) + +# ---[ Point Cloud Library - pcl/cuda/kdtree + +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}) +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_flann.h" +) + +set(impl_incs + "include/pcl/${SUBSYS_PATH}/impl/kdtree_flann.hpp" +) + +set(LIB_NAME "pcl_${SUBSYS_NAME}") +include_directories("${CMAKE_SOURCE_DIR}/kdtree/include" "${CMAKE_CURRENT_SOURCE_DIR}/include") +PCL_CUDA_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs}) +target_link_libraries("${LIB_NAME}" FLANN::FLANN) + +set(EXT_DEPS flann) +#set(EXT_DEPS 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}) 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..e9446504547 --- /dev/null +++ b/cuda/kdtree/include/pcl/cuda/kdtree/impl/kdtree_flann.hpp @@ -0,0 +1,12 @@ +#ifndef PCL_CUDA_KTREE_IMPL_FLANN_H_ +#define PCL_CUDA_KTREE_IMPL_FLANN_H_ + +#define FLANN_USE_CUDA + +#include +#include + + + +#endif + 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..10e91cc323f --- /dev/null +++ b/cuda/kdtree/include/pcl/cuda/kdtree/kdtree_flann.h @@ -0,0 +1,276 @@ +#pragma once + +#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 Comaptibility 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 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 FlannIndex& index, + const Query& query, + Indices& indices, + Distances& dists, + float radius, + const SearchParams& params); + +/** + * @brief Comaptibility 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 + * @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 Query& query, + 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 Radu B. Rusu, Marius Muja + * \ingroup kdtree + */ +template > +class KdTreeFLANN : public pcl::KdTree { +public: + using KdTree::input_; + using KdTree::indices_; + 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 FLANNIndex = ::flann::Index; + + // Boost 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. + * + * By setting sorted to false, the \ref radiusSearch operations will be faster. + */ + KdTreeFLANN(bool sorted = true); + + /** \brief Copy constructor + * \param[in] k the tree to copy into this + */ + KdTreeFLANN(const KdTreeFLANN& k); + + /** \brief Copy operator + * \param[in] k the tree to copy into this + */ + inline KdTreeFLANN& + operator=(const KdTreeFLANN& k) + { + KdTree::operator=(k); + flann_index_ = k.flann_index_; + cloud_ = k.cloud_; + index_mapping_ = k.index_mapping_; + identity_mapping_ = k.identity_mapping_; + dim_ = k.dim_; + total_nr_points_ = k.total_nr_points_; + param_k_ = k.param_k_; + param_radius_ = k.param_radius_; + return (*this); + } + + /** \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 KdTreeFLANN(*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 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 + */ + 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 (must be + * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared + * distances to the neighboring points (must be resized to \a k a priori!) \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. \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 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 ("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). */ + 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 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..ba1972278fe --- /dev/null +++ b/cuda/kdtree/src/kdtree_flann.cpp @@ -0,0 +1 @@ +#include diff --git a/kdtree/CMakeLists.txt b/kdtree/CMakeLists.txt index a5a0fb9b8ec..57c9742875a 100644 --- a/kdtree/CMakeLists.txt +++ b/kdtree/CMakeLists.txt @@ -20,17 +20,19 @@ set(incs "include/pcl/${SUBSYS_NAME}/kdtree.h" "include/pcl/${SUBSYS_NAME}/io.h" "include/pcl/${SUBSYS_NAME}/kdtree_flann.h" + "include/pcl/${SUBSYS_NAME}/kdtree_nanoflann.h" ) set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/io.hpp" "include/pcl/${SUBSYS_NAME}/impl/kdtree_flann.hpp" + "include/pcl/${SUBSYS_NAME}/impl/kdtree_nanoflann.hpp" ) 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) +target_link_libraries("${LIB_NAME}" pcl_common FLANN::FLANN nanoflann::nanoflann) set(EXT_DEPS flann) PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) diff --git a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp index d276dee8a17..662072fc186 100644 --- a/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp +++ b/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp @@ -139,6 +139,7 @@ pcl::KdTreeFLANN::setInputCloud (const PointCloudConstPtr &cloud, /////////////////////////////////////////////////////////////////////////////////////////// namespace pcl { +//namespace flann { namespace detail { // Replace using constexpr in C++17 template (index, query, indices, dists, k, params); } +//} // namespace flann } // namespace pcl template int @@ -255,7 +257,7 @@ pcl::KdTreeFLANN::nearestKSearch (const PointT &point, unsigned in // Wrap the k_distances vector (no data copy) ::flann::Matrix k_distances_mat (&k_distances[0], 1, k); - knn_search(*flann_index_, + pcl::knn_search(*flann_index_, ::flann::Matrix(&query[0], 1, dim_), k_indices, k_distances_mat, @@ -277,6 +279,7 @@ pcl::KdTreeFLANN::nearestKSearch (const PointT &point, unsigned in /////////////////////////////////////////////////////////////////////////////////////////// namespace pcl { +//namespace flann { namespace detail { // Replace using constexpr in C++17 template ( index, query, indices, dists, radius, params); } +//} // namespace flann } // namespace pcl template int @@ -393,7 +397,7 @@ pcl::KdTreeFLANN::radiusSearch (const PointT &point, double radius params.max_neighbors = max_nn; auto query_mat = ::flann::Matrix(&query[0], 1, dim_); - int neighbors_in_radius = radius_search(*flann_index_, + int neighbors_in_radius = pcl::radius_search(*flann_index_, query_mat, k_indices, dists, 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..54abb5ff3ef --- /dev/null +++ b/kdtree/include/pcl/kdtree/impl/kdtree_nanoflann.hpp @@ -0,0 +1,375 @@ +#ifndef PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ +#define PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ + +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::KdTreeNanoflann::KdTreeNanoflann (bool sorted, int leaf_max_size) + : pcl::KdTree (sorted) + , nanoflann_index_ () + , identity_mapping_ (false) + , dim_ (0), leaf_max_size_ (leaf_max_size), 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::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::setLeafMaxSize (int leaf_max_size) +{ + leaf_max_size_ = leaf_max_size; + + if (nanoflann_index_ != nullptr) + { + nanoflann_index_.reset(new NanoflannIndex(dim_, mat_, leaf_max_size_)); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////// +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; + } + + mat_ = Eigen::Map(cloud_.get (), index_mapping_.size (), dim_); + nanoflann_index_.reset(new NanoflannIndex(dim_, mat_, leaf_max_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); + + /*std::transform( + indices_dists.cbegin(), + indices_dists.cend(), + k_indices.begin(), + [](const auto& index_dist) + { return index_dist.first; } + ); + + std::transform( + indices_dists.cbegin(), + indices_dists.cend(), + dists.begin(), + [](const auto& index_dist) + { return index_dist.second; } + );*/ + + k_indices.clear(); + k_indices.reserve(indices_dists.size()); + + dists.clear(); + dists.reserve(indices_dists.size()); + + for (const auto& index_dist : indices_dists) + { + long index = index_dist.first; + 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 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::KdTreeNanoflann::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_KdTreeNanoflann(T) template class PCL_EXPORTS pcl::KdTreeNanoflann; + +#endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_NANOFLANN_H_ + diff --git a/kdtree/include/pcl/kdtree/kdtree_flann.h b/kdtree/include/pcl/kdtree/kdtree_flann.h index e41b96c7a29..9b83301937c 100644 --- a/kdtree/include/pcl/kdtree/kdtree_flann.h +++ b/kdtree/include/pcl/kdtree/kdtree_flann.h @@ -52,8 +52,8 @@ namespace flann template class Index; } -namespace pcl -{ +namespace pcl { +//namespace flann { namespace detail { // Helper struct to create a compatible Matrix and copy data back when needed // Replace using if constexpr in C++17 @@ -120,6 +120,7 @@ knn_search(const FlannIndex& index, 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) diff --git a/kdtree/include/pcl/kdtree/kdtree_nanoflann.h b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h new file mode 100644 index 00000000000..daee64c8bee --- /dev/null +++ b/kdtree/include/pcl/kdtree/kdtree_nanoflann.h @@ -0,0 +1,150 @@ +#pragma once + +#include +#include + +#include + +namespace pcl { +namespace nanoflann { +template +int +radius_search(const NanoflannIndex& index, + const Query& query, + Indices& indices, + Distances& dists, + float radius, + const SearchParams& params); + +template +int +knn_search(const NanoflannIndex& index, + const Query& query, + Indices& indices, + Distances& dists, + unsigned int k, + const SearchParams& params); +} // namespace nanoflann + +template +class KdTreeNanoflann : public pcl::KdTree { +public: + using KdTree::input_; + using KdTree::indices_; + 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; + + KdTreeNanoflann(bool sorted = true, int leaf_max_size = 15); + + KdTreeNanoflann(const Tree& k); + + inline Tree& + operator=(const Tree& k) + { + KdTree::operator=(k); + cloud_ = k.cloud_; + index_mapping_ = k.index_mapping_; + identity_mapping_ = k.identity_mapping_; + dim_ = k.dim_; + leaf_max_size_ = k.leaf_max_size_; + total_nr_points_ = k.total_nr_points_; + param_radius_ = k.param_radius_; + + if (k.nanoflann_index_ != nullptr) + { + mat_ = k.mat_; + nanoflann_index_.reset(new NanoflannIndex(dim_, mat_, leaf_max_size_)); + } + + return (*this); + } + + void + setEpsilon(float eps) override; + + void + setSortedResults(bool sorted); + + void + setLeafMaxSize(int leaf_max_size); + + inline Ptr + makeShared() + { + return Ptr(new Tree(*this)); + } + + ~KdTreeNanoflann() { cleanup(); } + + void + setInputCloud(const PointCloudConstPtr& cloud, + const IndicesConstPtr& indices = IndicesConstPtr()) override; + + int + nearestKSearch(const PointT& point, + unsigned int k, + Indices& k_indices, + std::vector& k_sqr_distances) const override; + + int + radiusSearch(const PointT& point, + double radius, + Indices& k_indices, + std::vector& k_sqr_distances, + unsigned int max_nn = 0) const override; + +private: + void + cleanup(); + + void + convertCloudToArray(const PointCloud& cloud); + + void + convertCloudToArray(const PointCloud& cloud, const Indices& indices); + + std::string + getName() const override + { + return ("KdTreeNanoflann"); + } + + std::shared_ptr nanoflann_index_; + std::shared_ptr cloud_; + std::vector index_mapping_; + bool identity_mapping_; + int dim_; + int leaf_max_size_; + uindex_t total_nr_points_; + ::nanoflann::SearchParams param_radius_; + Matrix mat_; +}; +} + +#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..3b5a5c5d47a --- /dev/null +++ b/kdtree/src/kdtree_nanoflann.cpp @@ -0,0 +1,9 @@ +#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/test_kdtree.cpp b/test/kdtree/test_kdtree.cpp index f0baf755c1e..0e9a9dc0650 100644 --- a/test/kdtree/test_kdtree.cpp +++ b/test/kdtree/test_kdtree.cpp @@ -37,6 +37,7 @@ */ #include +#include #include #include @@ -65,33 +66,51 @@ 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>, + std::tuple, KdTreeNanoflann> +>; +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,7 +142,7 @@ TEST (PCL, KdTreeFLANN_radiusSearch) EXPECT_FALSE (error); { - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (cloud_big.makeShared ()); ScopeTime scopeTime ("FLANN radiusSearch"); @@ -134,7 +153,7 @@ TEST (PCL, KdTreeFLANN_radiusSearch) } { - KdTreeFLANN kdtree; + Tree kdtree; kdtree.setInputCloud (cloud_big.makeShared ()); ScopeTime scopeTime ("FLANN radiusSearch (max neighbors in radius)"); @@ -146,7 +165,7 @@ TEST (PCL, KdTreeFLANN_radiusSearch) { - KdTreeFLANN kdtree (false); + Tree kdtree (false); kdtree.setInputCloud (cloud_big.makeShared ()); ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)"); @@ -158,9 +177,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; @@ -200,7 +224,7 @@ TEST (PCL, KdTreeFLANN_nearestKSearch) ScopeTime scopeTime ("FLANN 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 +247,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 +263,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 +283,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 +315,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; @@ -346,7 +374,6 @@ main (int argc, char** argv) testing::InitGoogleTest (&argc, argv); - init (); return (RUN_ALL_TESTS ()); } /* ]--- */