Skip to content

Commit cbfe661

Browse files
yasamokayasamoka
committed
added AABB tree with tests; updated CropHull to use AABB tree"
1 parent e05d427 commit cbfe661

File tree

14 files changed

+963
-94
lines changed

14 files changed

+963
-94
lines changed

‎aabb_tree/CMakeLists.txt

+55
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
set(SUBSYS_NAME aabb_tree)
2+
set(SUBSYS_DESC "Geometry AABB tree library")
3+
set(SUBSYS_DEPS common)
4+
set(EXT_DEPS "")
5+
6+
option(BUILD_aabb_tree "Build AABB tree making use of CGAL" ON)
7+
8+
if (NOT BUILD_aabb_tree)
9+
return()
10+
endif()
11+
12+
set(CGAL_DO_NOT_WARN_ABOUT_CMAKE_BUILD_TYPE TRUE)
13+
find_package(CGAL)
14+
if (NOT CGAL_FOUND)
15+
return()
16+
endif()
17+
18+
set(build TRUE)
19+
20+
PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON)
21+
PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS})
22+
23+
PCL_ADD_DOC("${SUBSYS_NAME}")
24+
25+
if(NOT build)
26+
return()
27+
endif()
28+
29+
set(srcs
30+
src/aabb_tree_cgal.cpp
31+
)
32+
33+
set(incs
34+
"include/pcl/${SUBSYS_NAME}/aabb_tree.h"
35+
"include/pcl/${SUBSYS_NAME}/aabb_tree_cgal.h"
36+
)
37+
38+
set(impl_incs
39+
"include/pcl/${SUBSYS_NAME}/impl/aabb_tree_cgal.hpp"
40+
)
41+
42+
set(libs
43+
pcl_common
44+
CGAL::CGAL
45+
)
46+
47+
set(LIB_NAME "pcl_${SUBSYS_NAME}")
48+
include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include")
49+
PCL_ADD_LIBRARY(${LIB_NAME} COMPONENT ${SUBSYS_NAME} SOURCES ${srcs} ${incs} ${impl_incs})
50+
target_link_libraries("${LIB_NAME}" ${libs})
51+
PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} PCL_DEPS ${SUBSYS_DEPS} EXT_DEPS ${EXT_DEPS})
52+
53+
# Install include files
54+
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs})
55+
PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs})

‎aabb_tree/aabb_tree.doxy

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
/**
2+
\addtogroup aabb_tree Module aabb_tree
3+
4+
\section secAABBTreePresentation Overview
5+
6+
The <b>pcl_aabb_tree</b> library provides the AABB tree data structure, using
7+
<a href="https://doc.cgal.org/latest/AABB_tree/index.html">CGAL</a>,
8+
that allows for fast <a href="https://en.wikipedia.org/wiki/Bounding_volume_hierarchy">ray intersection testing</a>.
9+
10+
\section secAABBTreeRequirements Requirements
11+
- \ref common "common"
12+
13+
*/

‎aabb_tree/include/pcl/aabb_tree/aabb_tree.h

+118
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Point Cloud Library (PCL) - www.pointclouds.org
5+
* Copyright (c) 2009-2011, Willow Garage, Inc.
6+
* Copyright (c) 2012-, Open Perception, Inc.
7+
*
8+
* All rights reserved.
9+
*
10+
* Redistribution and use in source and binary forms, with or without
11+
* modification, are permitted provided that the following conditions
12+
* are met:
13+
*
14+
* * Redistributions of source code must retain the above copyright
15+
* notice, this list of conditions and the following disclaimer.
16+
* * Redistributions in binary form must reproduce the above
17+
* copyright notice, this list of conditions and the following
18+
* disclaimer in the documentation and/or other materials provided
19+
* with the distribution.
20+
* * Neither the name of the copyright holder(s) nor the names of its
21+
* contributors may be used to endorse or promote products derived
22+
* from this software without specific prior written permission.
23+
*
24+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35+
* POSSIBILITY OF SUCH DAMAGE.
36+
*
37+
* aabb_tree.h
38+
* Created on: Jun 02, 2022
39+
* Author: Ramzi Sabra
40+
*/
41+
42+
#pragma once
43+
44+
#include <pcl/PolygonMesh.h>
45+
#include <pcl/point_cloud.h>
46+
#include <pcl/point_types.h>
47+
48+
namespace pcl {
49+
/** \brief AABBTree represents the base ray intersection testing class for AABB tree
50+
* implementations. \author Ramzi Sabra \ingroup aabb_tree
51+
*/
52+
template <typename PointT>
53+
class AABBTree {
54+
public:
55+
using PointCloud = pcl::PointCloud<PointT>;
56+
using PointCloudPtr = typename PointCloud::Ptr;
57+
using PointCloudConstPtr = std::shared_ptr<const PointCloud>;
58+
59+
virtual ~AABBTree(){};
60+
61+
/** \brief Provide an input mesh to construct the tree.
62+
* \param[in] mesh the polygon mesh
63+
*/
64+
virtual void
65+
setInputMesh(const pcl::PolygonMesh& mesh) = 0;
66+
67+
/** \brief Provide an input mesh to construct the tree.
68+
* \param[in] mesh_cloud the mesh cloud
69+
* \param[in] mesh_polygons the mesh polygons
70+
*/
71+
virtual void
72+
setInputMesh(PointCloudConstPtr mesh_cloud,
73+
const std::vector<Vertices>& mesh_polygons) = 0;
74+
75+
/** \brief Check for the presence of intersection(s) for the given ray
76+
* \param[in] source the ray source point
77+
* \param[in] direction the ray direction vector
78+
*/
79+
virtual bool
80+
checkForIntersection(const PointT& source,
81+
const Eigen::Vector3f& direction) const = 0;
82+
83+
/** \brief Check for the number of intersections for the given ray
84+
* \param[in] source the ray source point
85+
* \param[in] direction the ray direction vector
86+
*/
87+
virtual size_t
88+
numberOfIntersections(const PointT& source,
89+
const Eigen::Vector3f& direction) const = 0;
90+
91+
/** \brief Get all intersections for the given ray
92+
* \param[in] source the ray source point
93+
* \param[in] direction the ray direction vector
94+
*/
95+
virtual std::vector<index_t>
96+
getAllIntersections(const PointT& source, const Eigen::Vector3f& direction) const = 0;
97+
98+
/** \brief Get any intersection for the given ray
99+
* \param[in] source the ray source point
100+
* \param[in] direction the ray direction vector
101+
*/
102+
virtual index_t
103+
getAnyIntersection(const PointT& source, const Eigen::Vector3f& direction) const = 0;
104+
105+
/** \brief Get the nearest intersection for the given ray
106+
* \param[in] source the ray source point
107+
* \param[in] direction the ray direction vector
108+
*/
109+
virtual index_t
110+
getNearestIntersection(const PointT& source,
111+
const Eigen::Vector3f& direction) const = 0;
112+
113+
protected:
114+
/** \brief Class getName method. */
115+
virtual std::string
116+
getName() const = 0;
117+
};
118+
} // namespace pcl

‎aabb_tree/include/pcl/aabb_tree/aabb_tree_cgal.h

+169
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
/*
2+
* Software License Agreement (BSD License)
3+
*
4+
* Point Cloud Library (PCL) - www.pointclouds.org
5+
* Copyright (c) 2009-2011, Willow Garage, Inc.
6+
* Copyright (c) 2012-, Open Perception, Inc.
7+
*
8+
* All rights reserved.
9+
*
10+
* Redistribution and use in source and binary forms, with or without
11+
* modification, are permitted provided that the following conditions
12+
* are met:
13+
*
14+
* * Redistributions of source code must retain the above copyright
15+
* notice, this list of conditions and the following disclaimer.
16+
* * Redistributions in binary form must reproduce the above
17+
* copyright notice, this list of conditions and the following
18+
* disclaimer in the documentation and/or other materials provided
19+
* with the distribution.
20+
* * Neither the name of the copyright holder(s) nor the names of its
21+
* contributors may be used to endorse or promote products derived
22+
* from this software without specific prior written permission.
23+
*
24+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35+
* POSSIBILITY OF SUCH DAMAGE.
36+
*
37+
* aabb_tree_cgal.h
38+
* Created on: Jun 02, 2022
39+
* Author: Ramzi Sabra
40+
*/
41+
42+
#pragma once
43+
44+
#include <pcl/aabb_tree/aabb_tree.h>
45+
46+
#include <CGAL/AABB_traits.h>
47+
#include <CGAL/AABB_tree.h>
48+
#include <CGAL/AABB_triangle_primitive.h>
49+
#include <CGAL/Simple_cartesian.h>
50+
51+
namespace pcl {
52+
/** \brief AABBTreeCGAL is a ray intersection testing class using AABB tree
53+
* structures. The class is making use of the CGAL (The Computational Geometry Algorithms
54+
* Library) project. \author Ramzi Sabra \ingroup aabb_tree
55+
*/
56+
template <typename PointT>
57+
class AABBTreeCGAL : public AABBTree<PointT> {
58+
protected:
59+
using K = CGAL::Simple_cartesian<float>;
60+
using CGALPoint = K::Point_3;
61+
using CGALVector = K::Vector_3;
62+
using Ray = K::Ray_3;
63+
using Triangle = K::Triangle_3;
64+
65+
using Iterator = std::vector<Triangle>::const_iterator;
66+
using Primitive = CGAL::AABB_triangle_primitive<K, Iterator>;
67+
using AABB_triangle_traits = CGAL::AABB_traits<K, Primitive>;
68+
69+
public:
70+
using Tree = CGAL::AABB_tree<AABB_triangle_traits>;
71+
using TreePtr = std::shared_ptr<Tree>;
72+
using TreeConstPtr = std::shared_ptr<const Tree>;
73+
74+
using PointCloud = typename AABBTree<PointT>::PointCloud;
75+
using PointCloudPtr = typename AABBTree<PointT>::PointCloudPtr;
76+
using PointCloudConstPtr = typename AABBTree<PointT>::PointCloudConstPtr;
77+
78+
/** \brief Default constructor for AABBTreeCGAL.
79+
* \param[in] tree already-built CGAL AABB tree. Set to empty by default.
80+
*/
81+
AABBTreeCGAL(TreeConstPtr tree = TreeConstPtr());
82+
83+
/** \brief Set an already-build CGAL AABB tree as the tree.
84+
* \param[in] tree already-built CGAL AABB tree
85+
*/
86+
void
87+
setTree(TreeConstPtr tree);
88+
89+
/** \brief Provide an input mesh to construct the tree.
90+
* \param[in] mesh the polygon mesh
91+
*/
92+
void
93+
setInputMesh(const pcl::PolygonMesh& mesh) override;
94+
95+
/** \brief Provide an input mesh to construct the tree.
96+
* \param[in] mesh_cloud the mesh cloud
97+
* \param[in] mesh_polygons the mesh polygons
98+
*/
99+
void
100+
setInputMesh(PointCloudConstPtr mesh_cloud,
101+
const std::vector<Vertices>& mesh_polygons) override;
102+
103+
/** \brief Check for the presence of intersection(s) for the given ray
104+
* \param[in] source the ray source point
105+
* \param[in] direction the ray direction vector
106+
*/
107+
bool
108+
checkForIntersection(const PointT& source,
109+
const Eigen::Vector3f& direction) const override;
110+
111+
/** \brief Check for the number of intersections for the given ray
112+
* \param[in] source the ray source point
113+
* \param[in] direction the ray direction vector
114+
*/
115+
size_t
116+
numberOfIntersections(const PointT& source,
117+
const Eigen::Vector3f& direction) const override;
118+
119+
/** \brief Get all intersections for the given ray
120+
* \param[in] source the ray source point
121+
* \param[in] direction the ray direction vector
122+
*/
123+
std::vector<index_t>
124+
getAllIntersections(const PointT& source,
125+
const Eigen::Vector3f& direction) const override;
126+
127+
/** \brief Get any intersection for the given ray
128+
* \param[in] source the ray source point
129+
* \param[in] direction the ray direction vector
130+
*/
131+
index_t
132+
getAnyIntersection(const PointT& source,
133+
const Eigen::Vector3f& direction) const override;
134+
135+
/** \brief Get the nearest intersection for the given ray
136+
* \param[in] source the ray source point
137+
* \param[in] direction the ray direction vector
138+
*/
139+
index_t
140+
getNearestIntersection(const PointT& source,
141+
const Eigen::Vector3f& direction) const override;
142+
143+
protected:
144+
/** \brief Class getName method. */
145+
std::string
146+
getName() const override
147+
{
148+
return ("AABBTreeCGAL");
149+
}
150+
151+
/** \brief A CGAL AABB tree object. */
152+
TreeConstPtr tree_;
153+
154+
/** \brief CGAL triangles used for building the tree. */
155+
std::vector<Triangle> triangles_;
156+
157+
private:
158+
/** \brief Generate a CGAL ray from a source point and a direction vector
159+
* \param[in] source the ray source point
160+
* \param[in] direction the ray direction vector
161+
*/
162+
static Ray
163+
generateRay(const PointT& source, const Eigen::Vector3f& direction);
164+
};
165+
} // namespace pcl
166+
167+
#ifdef PCL_NO_PRECOMPILE
168+
#include <pcl/aabb_tree/impl/aabb_tree_cgal.hpp>
169+
#endif

0 commit comments

Comments
 (0)