added AABB tree with tests; updated CropHull to use AABB tree"

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})

aabb_tree/aabb_tree.doxy

@@ -0,0 +1,13 @@
+/**
+ \addtogroup aabb_tree Module aabb_tree
+
+ \section secAABBTreePresentation Overview 
+
+ The <b>pcl_aabb_tree</b> library provides the AABB tree data structure, using
+ <a href="https://doc.cgal.org/latest/AABB_tree/index.html">CGAL</a>,
+ that allows for fast <a href="https://en.wikipedia.org/wiki/Bounding_volume_hierarchy">ray intersection testing</a>.
+
+ \section secAABBTreeRequirements Requirements
+ - \ref common "common"
+
+*/

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 <pcl/PolygonMesh.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace pcl {
+/** \brief AABBTree represents the base ray intersection testing class for AABB tree
+ * implementations. \author Ramzi Sabra \ingroup aabb_tree
+ */
+template <typename PointT>
+class AABBTree {
+public:
+ using PointCloud = pcl::PointCloud<PointT>;
+ using PointCloudPtr = typename PointCloud::Ptr;
+ using PointCloudConstPtr = std::shared_ptr<const PointCloud>;
+
+ 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<Vertices>& 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<index_t>
+ 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

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 <pcl/aabb_tree/aabb_tree.h>
+
+#include <CGAL/AABB_traits.h>
+#include <CGAL/AABB_tree.h>
+#include <CGAL/AABB_triangle_primitive.h>
+#include <CGAL/Simple_cartesian.h>
+
+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 <typename PointT>
+class AABBTreeCGAL : public AABBTree<PointT> {
+protected:
+ using K = CGAL::Simple_cartesian<float>;
+ using CGALPoint = K::Point_3;
+ using CGALVector = K::Vector_3;
+ using Ray = K::Ray_3;
+ using Triangle = K::Triangle_3;
+
+ using Iterator = std::vector<Triangle>::const_iterator;
+ using Primitive = CGAL::AABB_triangle_primitive<K, Iterator>;
+ using AABB_triangle_traits = CGAL::AABB_traits<K, Primitive>;
+
+public:
+ using Tree = CGAL::AABB_tree<AABB_triangle_traits>;
+ using TreePtr = std::shared_ptr<Tree>;
+ using TreeConstPtr = std::shared_ptr<const Tree>;
+
+ using PointCloud = typename AABBTree<PointT>::PointCloud;
+ using PointCloudPtr = typename AABBTree<PointT>::PointCloudPtr;
+ using PointCloudConstPtr = typename AABBTree<PointT>::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<Vertices>& 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<index_t>
+ 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<Triangle> 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 <pcl/aabb_tree/impl/aabb_tree_cgal.hpp>
+#endif