CropHull::applyFilter3D speedup using 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