diff --git a/aabb_tree/CMakeLists.txt b/aabb_tree/CMakeLists.txt new file mode 100644 index 00000000000..a6fb9ad81fa --- /dev/null +++ b/aabb_tree/CMakeLists.txt @@ -0,0 +1,55 @@ +set(SUBSYS_NAME aabb_tree) +set(SUBSYS_DESC "Geometry AABB tree library") +set(SUBSYS_DEPS common) +set(EXT_DEPS "") + +option(BUILD_aabb_tree "Build AABB tree making use of CGAL" ON) + +if (NOT BUILD_aabb_tree) + return() +endif() + +set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE) +find_package(CGAL) +if (NOT CGAL_FOUND) + return() +endif() + +set(build TRUE) + +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS}) + +PCL_ADD_DOC("${SUBSYS_NAME}") + +if(NOT build) + return() +endif() + +set(srcs + src/aabb_tree_cgal.cpp +) + +set(incs + "include/pcl/${SUBSYS_NAME}/aabb_tree.h" + "include/pcl/${SUBSYS_NAME}/aabb_tree_cgal.h" +) + +set(impl_incs + "include/pcl/${SUBSYS_NAME}/impl/aabb_tree_cgal.hpp" +) + +set(libs + pcl_common + CGAL::CGAL +) + +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}" ${libs}) +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_NAME}" ${incs}) +PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) diff --git a/aabb_tree/aabb_tree.doxy b/aabb_tree/aabb_tree.doxy new file mode 100644 index 00000000000..bc7126fcb7c --- /dev/null +++ b/aabb_tree/aabb_tree.doxy @@ -0,0 +1,13 @@ +/** + \addtogroup aabb_tree Module aabb_tree + + \section secAABBTreePresentation Overview + + The pcl_aabb_tree library provides the AABB tree data structure, using + CGAL, + that allows for fast ray intersection testing. + + \section secAABBTreeRequirements Requirements + - \ref common "common" + +*/ diff --git a/aabb_tree/include/pcl/aabb_tree/aabb_tree.h b/aabb_tree/include/pcl/aabb_tree/aabb_tree.h new file mode 100644 index 00000000000..96d980be65a --- /dev/null +++ b/aabb_tree/include/pcl/aabb_tree/aabb_tree.h @@ -0,0 +1,118 @@ +/* + * 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. + * + * aabb_tree.h + * Created on: Jun 02, 2022 + * Author: Ramzi Sabra + */ + +#pragma once + +#include +#include +#include + +namespace pcl { +/** \brief AABBTree represents the base ray intersection testing class for AABB tree + * implementations. \author Ramzi Sabra \ingroup aabb_tree + */ +template +class AABBTree { +public: + using PointCloud = pcl::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = std::shared_ptr; + + virtual ~AABBTree(){}; + + /** \brief Provide an input mesh to construct the tree. + * \param[in] mesh the polygon mesh + */ + virtual void + setInputMesh(const pcl::PolygonMesh& mesh) = 0; + + /** \brief Provide an input mesh to construct the tree. + * \param[in] mesh_cloud the mesh cloud + * \param[in] mesh_polygons the mesh polygons + */ + virtual void + setInputMesh(PointCloudConstPtr mesh_cloud, + const std::vector& mesh_polygons) = 0; + + /** \brief Check for the presence of intersection(s) for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + virtual bool + checkForIntersection(const PointT& source, + const Eigen::Vector3f& direction) const = 0; + + /** \brief Check for the number of intersections for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + virtual size_t + numberOfIntersections(const PointT& source, + const Eigen::Vector3f& direction) const = 0; + + /** \brief Get all intersections for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + virtual std::vector + getAllIntersections(const PointT& source, const Eigen::Vector3f& direction) const = 0; + + /** \brief Get any intersection for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + virtual index_t + getAnyIntersection(const PointT& source, const Eigen::Vector3f& direction) const = 0; + + /** \brief Get the nearest intersection for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + virtual index_t + getNearestIntersection(const PointT& source, + const Eigen::Vector3f& direction) const = 0; + +protected: + /** \brief Class getName method. */ + virtual std::string + getName() const = 0; +}; +} // namespace pcl diff --git a/aabb_tree/include/pcl/aabb_tree/aabb_tree_cgal.h b/aabb_tree/include/pcl/aabb_tree/aabb_tree_cgal.h new file mode 100644 index 00000000000..86ff7f23ae9 --- /dev/null +++ b/aabb_tree/include/pcl/aabb_tree/aabb_tree_cgal.h @@ -0,0 +1,169 @@ +/* + * 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. + * + * aabb_tree_cgal.h + * Created on: Jun 02, 2022 + * Author: Ramzi Sabra + */ + +#pragma once + +#include + +#include +#include +#include +#include + +namespace pcl { +/** \brief AABBTreeCGAL is a ray intersection testing class using AABB tree + * structures. The class is making use of the CGAL (The Computational Geometry Algorithms + * Library) project. \author Ramzi Sabra \ingroup aabb_tree + */ +template +class AABBTreeCGAL : public AABBTree { +protected: + using K = CGAL::Simple_cartesian; + using CGALPoint = K::Point_3; + using CGALVector = K::Vector_3; + using Ray = K::Ray_3; + using Triangle = K::Triangle_3; + + using Iterator = std::vector::const_iterator; + using Primitive = CGAL::AABB_triangle_primitive; + using AABB_triangle_traits = CGAL::AABB_traits; + +public: + using Tree = CGAL::AABB_tree; + using TreePtr = std::shared_ptr; + using TreeConstPtr = std::shared_ptr; + + using PointCloud = typename AABBTree::PointCloud; + using PointCloudPtr = typename AABBTree::PointCloudPtr; + using PointCloudConstPtr = typename AABBTree::PointCloudConstPtr; + + /** \brief Default constructor for AABBTreeCGAL. + * \param[in] tree already-built CGAL AABB tree. Set to empty by default. + */ + AABBTreeCGAL(TreeConstPtr tree = TreeConstPtr()); + + /** \brief Set an already-build CGAL AABB tree as the tree. + * \param[in] tree already-built CGAL AABB tree + */ + void + setTree(TreeConstPtr tree); + + /** \brief Provide an input mesh to construct the tree. + * \param[in] mesh the polygon mesh + */ + void + setInputMesh(const pcl::PolygonMesh& mesh) override; + + /** \brief Provide an input mesh to construct the tree. + * \param[in] mesh_cloud the mesh cloud + * \param[in] mesh_polygons the mesh polygons + */ + void + setInputMesh(PointCloudConstPtr mesh_cloud, + const std::vector& mesh_polygons) override; + + /** \brief Check for the presence of intersection(s) for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + bool + checkForIntersection(const PointT& source, + const Eigen::Vector3f& direction) const override; + + /** \brief Check for the number of intersections for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + size_t + numberOfIntersections(const PointT& source, + const Eigen::Vector3f& direction) const override; + + /** \brief Get all intersections for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + std::vector + getAllIntersections(const PointT& source, + const Eigen::Vector3f& direction) const override; + + /** \brief Get any intersection for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + index_t + getAnyIntersection(const PointT& source, + const Eigen::Vector3f& direction) const override; + + /** \brief Get the nearest intersection for the given ray + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + index_t + getNearestIntersection(const PointT& source, + const Eigen::Vector3f& direction) const override; + +protected: + /** \brief Class getName method. */ + std::string + getName() const override + { + return ("AABBTreeCGAL"); + } + + /** \brief A CGAL AABB tree object. */ + TreeConstPtr tree_; + + /** \brief CGAL triangles used for building the tree. */ + std::vector triangles_; + +private: + /** \brief Generate a CGAL ray from a source point and a direction vector + * \param[in] source the ray source point + * \param[in] direction the ray direction vector + */ + static Ray + generateRay(const PointT& source, const Eigen::Vector3f& direction); +}; +} // namespace pcl + +#ifdef PCL_NO_PRECOMPILE +#include +#endif diff --git a/aabb_tree/include/pcl/aabb_tree/impl/aabb_tree_cgal.hpp b/aabb_tree/include/pcl/aabb_tree/impl/aabb_tree_cgal.hpp new file mode 100644 index 00000000000..880570e6982 --- /dev/null +++ b/aabb_tree/include/pcl/aabb_tree/impl/aabb_tree_cgal.hpp @@ -0,0 +1,190 @@ +/* + * 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. + * + * aabb_tree_cgal.cpp + * Created on: Jun 02, 2022 + * Author: Ramzi Sabra + */ + +#ifndef PCL_AABB_TREE_AABB_TREE_IMPL_CGAL_H_ +#define PCL_AABB_TREE_AABB_TREE_IMPL_CGAL_H_ + +#include +#include +#include +#include + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::AABBTreeCGAL::AABBTreeCGAL(TreeConstPtr tree) : tree_(tree) +{} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::AABBTreeCGAL::setTree(TreeConstPtr tree) +{ + tree_ = tree; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::AABBTreeCGAL::setInputMesh(const pcl::PolygonMesh& mesh) +{ + PointCloudPtr mesh_cloud = std::make_shared(); + pcl::fromPCLPointCloud2(mesh.cloud, *mesh_cloud); + setInputMesh(mesh_cloud, mesh.polygons); +} + +template +void +pcl::AABBTreeCGAL::setInputMesh(PointCloudConstPtr mesh_cloud, + const std::vector& mesh_polygons) +{ +#if DEBUG + for (const pcl::Vertices& vertices : mesh_polygons) + assert(vertices.vertices.size() == 3 && + "Invalid number of vertices for polygon (should be 3)!"); +#endif + + triangles_.resize(mesh_polygons.size()); + std::transform(mesh_polygons.cbegin(), + mesh_polygons.cend(), + triangles_.begin(), + [mesh_cloud](const Vertices& vertices) { + const pcl::Indices& indices = vertices.vertices; + + std::array cgal_points; + std::transform(indices.cbegin(), + indices.cend(), + cgal_points.begin(), + [mesh_cloud](const index_t& index) { + const PointT& point = mesh_cloud->points[index]; + return CGALPoint(point.x, point.y, point.z); + }); + + return Triangle(cgal_points[0], cgal_points[1], cgal_points[2]); + }); + + tree_.reset(new Tree(triangles_.cbegin(), triangles_.cend())); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +bool +pcl::AABBTreeCGAL::checkForIntersection(const PointT& source, + const Eigen::Vector3f& direction) const +{ + Ray ray = generateRay(source, direction); + return tree_->do_intersect(ray); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +size_t +pcl::AABBTreeCGAL::numberOfIntersections(const PointT& source, + const Eigen::Vector3f& direction) const +{ + Ray ray = generateRay(source, direction); + return tree_->number_of_intersected_primitives(ray); +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +std::vector +pcl::AABBTreeCGAL::getAllIntersections(const PointT& source, + const Eigen::Vector3f& direction) const +{ + Ray ray = generateRay(source, direction); + std::vector primitive_ids; + tree_->all_intersected_primitives(ray, std::back_inserter(primitive_ids)); + + std::vector triangle_indices(primitive_ids.size()); + Iterator first_primitive_id = triangles_.cbegin(); + + std::transform(primitive_ids.cbegin(), + primitive_ids.cend(), + triangle_indices.begin(), + [&first_primitive_id](const Iterator& primitive_id) { + return std::distance(first_primitive_id, primitive_id); + }); + + return triangle_indices; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::index_t +pcl::AABBTreeCGAL::getAnyIntersection(const PointT& source, + const Eigen::Vector3f& direction) const +{ + Ray ray = generateRay(source, direction); + auto primitive_id = tree_->any_intersected_primitive(ray); + + if (primitive_id.has_value()) + return std::distance(triangles_.cbegin(), primitive_id.value()); + return -1; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +pcl::index_t +pcl::AABBTreeCGAL::getNearestIntersection( + const PointT& source, const Eigen::Vector3f& direction) const +{ + Ray ray = generateRay(source, direction); + auto primitive_id = tree_->first_intersected_primitive(ray); + + if (primitive_id.has_value()) + return std::distance(triangles_.cbegin(), primitive_id.value()); + return -1; +} + +/////////////////////////////////////////////////////////////////////////////////////////// +template +typename pcl::AABBTreeCGAL::Ray +pcl::AABBTreeCGAL::generateRay(const PointT& source, + const Eigen::Vector3f& direction) +{ + CGALPoint cgal_source(source.x, source.y, source.z); + CGALVector cgal_direction(direction[0], direction[1], direction[2]); + return Ray(cgal_source, cgal_direction); +} + +#define PCL_INSTANTIATE_AABBTreeCGAL(T) template class PCL_EXPORTS pcl::AABBTreeCGAL; + +#endif //#ifndef _PCL_AABB_TREE_AABB_TREE_IMPL_CGAL_H_ diff --git a/aabb_tree/src/aabb_tree_cgal.cpp b/aabb_tree/src/aabb_tree_cgal.cpp new file mode 100644 index 00000000000..0ae09b29c18 --- /dev/null +++ b/aabb_tree/src/aabb_tree_cgal.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(AABBTreeCGAL, PCL_XYZ_POINT_TYPES) +#endif // PCL_NO_PRECOMPILE + diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index bb91d7bb179..9e436019325 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -1,6 +1,6 @@ set(SUBSYS_NAME filters) set(SUBSYS_DESC "Point cloud filters library") -set(SUBSYS_DEPS common sample_consensus search kdtree octree) +set(SUBSYS_DEPS common sample_consensus search kdtree octree aabb_tree) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) @@ -130,7 +130,7 @@ set(impl_incs 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 pcl_sample_consensus pcl_search pcl_kdtree pcl_octree) +target_link_libraries("${LIB_NAME}" pcl_common pcl_sample_consensus pcl_search pcl_kdtree pcl_octree pcl_aabb_tree) PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS}) # Install include files diff --git a/filters/include/pcl/filters/crop_hull.h b/filters/include/pcl/filters/crop_hull.h index 59ed4b5bcc9..07e66d5a37c 100644 --- a/filters/include/pcl/filters/crop_hull.h +++ b/filters/include/pcl/filters/crop_hull.h @@ -39,6 +39,7 @@ #include #include +#include namespace pcl { @@ -67,8 +68,10 @@ namespace pcl /** \brief Empty Constructor. */ CropHull () : hull_cloud_(), + tree_dirty_(true), dim_(3), - crop_outside_(true) + crop_outside_(true), + num_threads_(1) { filter_name_ = "CropHull"; } @@ -81,6 +84,7 @@ namespace pcl setHullIndices (const std::vector& polygons) { hull_polygons_ = polygons; + tree_dirty_ = true; } /** \brief Get the vertices of the hull used to filter points. @@ -98,6 +102,7 @@ namespace pcl setHullCloud (PointCloudPtr points) { hull_cloud_ = points; + tree_dirty_ = true; } /** \brief Get the point cloud that the hull indices refer to. */ @@ -106,6 +111,10 @@ namespace pcl { return (hull_cloud_); } + + /** \brief Build the AABB tree. */ + void + buildTree (); /** \brief Set the dimensionality of the hull to be used. * This should be set to correspond to the dimensionality of the @@ -129,6 +138,22 @@ namespace pcl crop_outside_ = crop_outside; } + /** \brief Set the number of threads to be used. + * \param[in] num_threads The number of threads + */ + inline void + setNumberOfThreads(unsigned int num_threads) + { + if (num_threads == 0) +#ifdef _OPENMP + num_threads_ = omp_get_num_procs(); +#else + num_threads_ = 1; +#endif + else + num_threads_ = num_threads; + } + protected: /** \brief Filter the input points using the 2D or 3D polygon hull. * \param[out] output The set of points that passed the filter @@ -182,20 +207,6 @@ namespace pcl const Vertices& verts, const PointCloud& cloud); - /** \brief Does a ray cast from a point intersect with an arbitrary - * triangle in 3D? - * See: http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() - * \param[in] point Point from which the ray is cast. - * \param[in] ray Vector in direction of ray. - * \param[in] verts Indices of vertices making the polygon. - * \param[in] cloud Cloud from which the vertex indices are drawn. - */ - inline static bool - rayTriangleIntersect (const PointT& point, - const Eigen::Vector3f& ray, - const Vertices& verts, - const PointCloud& cloud); - /** \brief The vertices of the hull used to filter points. */ std::vector hull_polygons_; @@ -203,6 +214,10 @@ namespace pcl /** \brief The point cloud that the hull indices refer to. */ PointCloudPtr hull_cloud_; + AABBTreeCGAL tree_; + + bool tree_dirty_; + /** \brief The dimensionality of the hull to be used. */ int dim_; @@ -210,6 +225,8 @@ namespace pcl * false, those inside will be removed. */ bool crop_outside_; + + unsigned int num_threads_; }; } // namespace pcl diff --git a/filters/include/pcl/filters/impl/crop_hull.hpp b/filters/include/pcl/filters/impl/crop_hull.hpp index 72d5000dc48..b97c8667c16 100644 --- a/filters/include/pcl/filters/impl/crop_hull.hpp +++ b/filters/include/pcl/filters/impl/crop_hull.hpp @@ -40,6 +40,17 @@ #include +#include + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +void +pcl::CropHull::buildTree () +{ + tree_.setInputMesh(hull_cloud_, hull_polygons_); + tree_dirty_ = false; +} ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template @@ -140,39 +151,56 @@ pcl::CropHull::applyFilter2D (Indices &indices) } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::CropHull::applyFilter3D (Indices &indices) +template +void +pcl::CropHull::applyFilter3D(Indices& indices) { - // This algorithm could definitely be sped up using kdtree/octree - // information, if that is available! + if (tree_dirty_) + buildTree(); - for (std::size_t index = 0; index < indices_->size (); index++) - { - // test ray-crossings for three random rays, and take vote of crossings - // counts to determine if each point is inside the hull: the vote avoids - // tricky edge and corner cases when rays might fluke through the edge - // between two polygons - // 'random' rays are arbitrary - basically anything that is less likely to - // hit the edge between polygons than coordinate-axis aligned rays would - // be. - std::size_t crossings[3] = {0,0,0}; - Eigen::Vector3f rays[3] = - { - Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f), + std::array directions = { + Eigen::Vector3f(0.264882f, 0.688399f, 0.675237f), Eigen::Vector3f(0.0145419f, 0.732901f, 0.68018f), - Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f) - }; + Eigen::Vector3f(0.856514f, 0.508771f, 0.0868081f)}; + + std::vector> crosses_vec(indices_->size()); + + { + // clang-format off +#pragma omp parallel for \ + default(none) \ + shared(directions, crosses_vec) \ + num_threads(num_threads_) + // clang-format on + for (int i = 0; i < static_cast(indices_->size()); ++i) { + const index_t& index = (*indices_)[i]; + std::atomic& crosses = crosses_vec[i]; + + const PointT& point = (*input_)[index]; + + std::array crossings; + std::transform(directions.cbegin(), + directions.cend(), + crossings.begin(), + [this, &point](const auto& direction) { + return tree_.numberOfIntersections(point, direction); + }); + + crosses = (crossings[0] & 1) + (crossings[1] & 1) + (crossings[2] & 1) > 1; + } + } + + auto index_itr = indices_->cbegin(); + auto crosses_itr = crosses_vec.cbegin(); - for (std::size_t poly = 0; poly < hull_polygons_.size (); poly++) - for (std::size_t ray = 0; ray < 3; ray++) - crossings[ray] += rayTriangleIntersect - ((*input_)[(*indices_)[index]], rays[ray], hull_polygons_[poly], *hull_cloud_); + for (; index_itr != indices_->cend(); ++index_itr, ++crosses_itr) { + const auto& index = *index_itr; + const auto& crosses = *crosses_itr; - bool crosses = (crossings[0]&1) + (crossings[1]&1) + (crossings[2]&1) > 1; if ((crop_outside_ && crosses) || (!crop_outside_ && !crosses)) - indices.push_back ((*indices_)[index]); + indices.push_back(index); else - removed_indices_->push_back ((*indices_)[index]); + removed_indices_->push_back(index); } } @@ -218,55 +246,6 @@ pcl::CropHull::isPointIn2DPolyWithVertIndices ( return (in_poly); } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -template bool -pcl::CropHull::rayTriangleIntersect (const PointT& point, - const Eigen::Vector3f& ray, - const Vertices& verts, - const PointCloud& cloud) -{ - // Algorithm here is adapted from: - // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() - // - // Original copyright notice: - // Copyright 2001, softSurfer (www.softsurfer.com) - // This code may be freely used and modified for any purpose - // providing that this copyright notice is included with it. - // - assert (verts.vertices.size () == 3); - - const Eigen::Vector3f p = point.getVector3fMap (); - const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap (); - const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap (); - const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap (); - const Eigen::Vector3f u = b - a; - const Eigen::Vector3f v = c - a; - const Eigen::Vector3f n = u.cross (v); - const float n_dot_ray = n.dot (ray); - - if (std::fabs (n_dot_ray) < 1e-9) - return (false); - - const float r = n.dot (a - p) / n_dot_ray; - - if (r < 0) - return (false); - - const Eigen::Vector3f w = p + r * ray - a; - const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v); - const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u); - const float s = s_numerator / denominator; - if (s < 0 || s > 1) - return (false); - - const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v); - const float t = t_numerator / denominator; - if (t < 0 || s+t > 1) - return (false); - - return (true); -} - #define PCL_INSTANTIATE_CropHull(T) template class PCL_EXPORTS pcl::CropHull; #endif // PCL_FILTERS_IMPL_CROP_HULL_H_ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cb7f77530de..9842d7ef251 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -47,6 +47,7 @@ set_target_properties(tests PROPERTIES FOLDER "Tests") include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include") add_subdirectory(2d) +add_subdirectory(aabb_tree) add_subdirectory(common) add_subdirectory(features) add_subdirectory(filters) diff --git a/test/aabb_tree/CMakeLists.txt b/test/aabb_tree/CMakeLists.txt new file mode 100644 index 00000000000..fe7210e2d5f --- /dev/null +++ b/test/aabb_tree/CMakeLists.txt @@ -0,0 +1,18 @@ +set(SUBSYS_NAME tests_aabb_tree) +set(SUBSYS_DESC "Point cloud library AABB tree module unit tests") +PCL_SET_TEST_DEPENDENCIES(SUBSYS_DEPS aabb_tree vtk) +set(OPT_DEPS io) # io is not a mandatory dependency in aabb_tree + +set(DEFAULT ON) +set(build TRUE) +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 (aabb_tree_aabb_tree test_aabb_tree + FILES test_aabb_tree.cpp + LINK_WITH pcl_gtest pcl_aabb_tree pcl_io pcl_common + ARGUMENTS "${PCL_SOURCE_DIR}/test/tum_rabbit.vtk") diff --git a/test/aabb_tree/test_aabb_tree.cpp b/test/aabb_tree/test_aabb_tree.cpp new file mode 100644 index 00000000000..6bfef9f8518 --- /dev/null +++ b/test/aabb_tree/test_aabb_tree.cpp @@ -0,0 +1,220 @@ +/* + * 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_aabb_tree.cpp + * Adapted from: test/kdtree/test_kdtree.cpp + * filters/impl/crop_hull.hpp + * Created on: Jun 05, 2022 + * Author: Ramzi Sabra + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include // For debug +#include +#include + +using namespace pcl; + +using Point = PointXYZ; +using Cloud = PointCloud; + +boost::property_tree::ptree xml_property_tree; + +PolygonMesh mesh_in; +Cloud mesh_cloud; + +bool +rayTriangleIntersect(const Point& point, + const Eigen::Vector3f& ray, + const Vertices& verts, + const Cloud& cloud) +{ + // Algorithm here is adapted from: + // http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle() + // + // Original copyright notice: + // Copyright 2001, softSurfer (www.softsurfer.com) + // This code may be freely used and modified for any purpose + // providing that this copyright notice is included with it. + // + assert(verts.vertices.size() == 3); + + const Eigen::Vector3f p = point.getVector3fMap(); + const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap(); + const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap(); + const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap(); + const Eigen::Vector3f u = b - a; + const Eigen::Vector3f v = c - a; + const Eigen::Vector3f n = u.cross(v); + const float n_dot_ray = n.dot(ray); + + if (std::fabs(n_dot_ray) < 1e-9) + return (false); + + const float r = n.dot(a - p) / n_dot_ray; + + if (r < 0) + return (false); + + const Eigen::Vector3f w = p + r * ray - a; + const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v); + const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u); + const float s = s_numerator / denominator; + if (s < 0 || s > 1) + return (false); + + const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v); + const float t = t_numerator / denominator; + if (t < 0 || s + t > 1) + return (false); + + return (true); +} + +template +class PCLAABBTreeTestFixture : public ::testing::Test { +public: + using Tree = T; +}; + +using AABBTreeTestTypes = ::testing::Types>; +TYPED_TEST_SUITE(PCLAABBTreeTestFixture, AABBTreeTestTypes); + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TYPED_TEST(PCLAABBTreeTestFixture, AABBTree_intersections) +{ + using Tree = typename TestFixture::Tree; + using Ray = std::pair; + + std::vector rays{ + {Point(0.0f, 70.0f, 0.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f)}, // 0 intersections + {Point(0.0f, 70.0f, 0.0f), Eigen::Vector3f(1.0f, 1.0f, 1.0f)}, // 2 intersections + {Point(70.0f, 100.0f, 80.0f), Eigen::Vector3f(1.0f, 0.0f, 0.0f)} // 1 intersection + }; + + std::vector triangle_indices_vec(rays.size()); + + std::transform( + rays.cbegin(), rays.cend(), triangle_indices_vec.begin(), [](const Ray& ray) { + const Point& source = ray.first; + const Eigen::Vector3f& direction = ray.second; + + Indices indices; + + for (index_t index = 0; index < mesh_in.polygons.size(); ++index) { + const Vertices& vertices = mesh_in.polygons[index]; + if (rayTriangleIntersect(source, direction, vertices, mesh_cloud)) + indices.push_back(index); + } + + return indices; + }); + + Tree tree; + tree.setInputMesh(mesh_in); + + auto ray_itr = rays.cbegin(); + auto triangle_indices_itr = triangle_indices_vec.cbegin(); + + for (; ray_itr != rays.cend(); ++ray_itr, ++triangle_indices_itr) { + const Ray& ray = *ray_itr; + const Indices& triangle_indices = *triangle_indices_itr; + + const Point& source = ray.first; + const Eigen::Vector3f& direction = ray.second; + + EXPECT_EQ(tree.checkForIntersection(source, direction), + triangle_indices.size() != 0); + EXPECT_EQ(tree.numberOfIntersections(source, direction), triangle_indices.size()); + + std::unordered_set triangle_indices_set, tree_triangle_indices_set; + + { + auto inserter = std::inserter(triangle_indices_set, triangle_indices_set.end()); + std::copy(triangle_indices.cbegin(), triangle_indices.cend(), inserter); + } + + { + Indices tree_triangle_indices = tree.getAllIntersections(source, direction); + auto inserter = + std::inserter(tree_triangle_indices_set, tree_triangle_indices_set.end()); + std::copy(tree_triangle_indices.cbegin(), tree_triangle_indices.cend(), inserter); + } + + EXPECT_EQ(tree_triangle_indices_set, triangle_indices_set); + + index_t any_triangle_index = tree.getAnyIntersection(source, direction); + index_t nearest_triangle_index = tree.getNearestIntersection(source, direction); + + if (triangle_indices_set.size() == 0) { + EXPECT_EQ(any_triangle_index, -1); + EXPECT_EQ(nearest_triangle_index, -1); + } + else { + EXPECT_TRUE(triangle_indices_set.count(any_triangle_index) == 1); + EXPECT_TRUE(triangle_indices_set.count(nearest_triangle_index) == 1); + } + } +} + +/* ---[ */ +int +main(int argc, char** argv) +{ + // Load the standard PLY file from disk + if (argc < 2) { + std::cerr << "No test file given. Please download `tum_rabbit.vtk` and pass its " + "path to the test." + << std::endl; + return (-1); + } + + // Load in the mesh + io::loadPolygonFileVTK(argv[1], mesh_in); + pcl::fromPCLPointCloud2(mesh_in.cloud, mesh_cloud); + + testing::InitGoogleTest(&argc, argv); + + return (RUN_ALL_TESTS()); +} +/* ]--- */ diff --git a/test/filters/test_crop_hull.cpp b/test/filters/test_crop_hull.cpp index ebe169ae65c..1fdf83e6018 100644 --- a/test/filters/test_crop_hull.cpp +++ b/test/filters/test_crop_hull.cpp @@ -314,6 +314,48 @@ TYPED_TEST (PCLCropHullTestFixture, simple_test) } } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TYPED_TEST (PCLCropHullTestFixture, build_tree) +{ + for (auto & entry : this->data_) + { + auto & crop_hull_filter = entry.first; + for (TestData const & test_data : entry.second) + { + crop_hull_filter.setInputCloud(test_data.input_cloud_); + crop_hull_filter.buildTree(); + pcl::Indices filtered_indices; + crop_hull_filter.filter(filtered_indices); + ASSERT_EQ(test_data.inside_indices_.size(), filtered_indices.size()); + pcl::test::EXPECT_EQ_VECTORS(test_data.inside_indices_, filtered_indices); + } + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TYPED_TEST (PCLCropHullTestFixture, build_tree_change_hull_cloud_and_indices) +{ + auto & entry = this->data_[0]; + auto & crop_hull_filter = entry.first; + + auto crop_hull_filter_copy = crop_hull_filter; + auto empty_hull_cloud = std::make_shared>(); + std::vector empty_hull_indices; + + { + crop_hull_filter.setHullCloud(empty_hull_cloud); + pcl::Indices filtered_indices; + crop_hull_filter.filter(filtered_indices); + ASSERT_EQ(filtered_indices.size(), 0); + } + + { + crop_hull_filter_copy.setHullIndices(empty_hull_indices); + pcl::Indices filtered_indices; + crop_hull_filter_copy.filter(filtered_indices); + ASSERT_EQ(filtered_indices.size(), 0); + } +} //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // checking that the result is independent of the original state of the output_indices diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt index 7f61854aee0..90b843a73c1 100644 --- a/tools/CMakeLists.txt +++ b/tools/CMakeLists.txt @@ -1,6 +1,6 @@ set(SUBSYS_NAME tools) set(SUBSYS_DESC "Useful PCL-based command line tools") -set(SUBSYS_DEPS common io filters sample_consensus segmentation search kdtree features surface octree registration recognition geometry keypoints ml) +set(SUBSYS_DEPS common io filters sample_consensus segmentation search kdtree features surface octree aabb_tree registration recognition geometry keypoints ml) set(SUBSYS_OPT_DEPS vtk visualization) set(DEFAULT ON) set(REASON "") @@ -163,7 +163,7 @@ endif() if(QHULL_FOUND) PCL_ADD_EXECUTABLE(pcl_crop_to_hull COMPONENT ${SUBSYS_NAME} SOURCES crop_to_hull.cpp) - target_link_libraries(pcl_crop_to_hull pcl_common pcl_io pcl_filters pcl_surface) + target_link_libraries(pcl_crop_to_hull pcl_common pcl_io pcl_filters pcl_surface pcl_aabb_tree) PCL_ADD_EXECUTABLE(pcl_compute_hull COMPONENT ${SUBSYS_NAME} SOURCES compute_hull.cpp) target_link_libraries(pcl_compute_hull pcl_common pcl_io pcl_surface)