Skip to content

Commit 8c2b414

Browse files
benoit2776benoit2776
committed
feat(OG): implementation of the dynamic KDTree of the nanoflann lib
1 parent c2aab5d commit 8c2b414

File tree

8 files changed

+509
-11
lines changed

8 files changed

+509
-11
lines changed

‎include/geode/geometry/dynamic_nn_search.hpp‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
/*
2+
* Copyright (c) 2019 - 2024 Geode-solutions
3+
*
4+
* Permission is hereby granted, free of charge, to any person obtaining a copy
5+
* of this software and associated documentation files (the "Software"), to deal
6+
* in the Software without restriction, including without limitation the rights
7+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8+
* copies of the Software, and to permit persons to whom the Software is
9+
* furnished to do so, subject to the following conditions:
10+
*
11+
* The above copyright notice and this permission notice shall be included in
12+
* all copies or substantial portions of the Software.
13+
*
14+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20+
* SOFTWARE.
21+
*
22+
*/
23+
24+
#pragma once
25+
26+
#include <geode/basic/pimpl.hpp>
27+
28+
#include <geode/geometry/common.hpp>
29+
#include <geode/geometry/point.hpp>
30+
31+
namespace geode
32+
{
33+
/*!
34+
* Given a list of points, this class returns neighboring points.
35+
*/
36+
template < index_t dimension >
37+
class DynamicNNSearch
38+
{
39+
OPENGEODE_DISABLE_COPY( DynamicNNSearch );
40+
// OPENGEODE_TEMPLATE_ASSERT_2D_OR_3D( DynamicNNSearch );
41+
42+
public:
43+
explicit DynamicNNSearch( std::vector< Point< dimension > > points );
44+
DynamicNNSearch( DynamicNNSearch&& other ) noexcept;
45+
~DynamicNNSearch();
46+
47+
index_t nb_points() const;
48+
49+
const Point< dimension >& point( index_t index ) const;
50+
51+
/*!
52+
* Get the neighbors closer than a given distance from the given point
53+
* or within a sphere
54+
* @param[in] point The center of the sphere
55+
* @param[in] threshold_distance The radius of the sphere
56+
* @return the list of points inside this distance
57+
*/
58+
std::vector< index_t > radius_neighbors(
59+
const Point< dimension >& point, double threshold_distance ) const;
60+
61+
void addPoint( const Point< dimension >& point );
62+
63+
private:
64+
IMPLEMENTATION_MEMBER( impl_ );
65+
};
66+
ALIAS_2D_AND_3D( DynamicNNSearch );
67+
} // namespace geode

‎src/geode/geometry/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@ add_geode_library(
3838
"common.cpp"
3939
"coordinate_system.cpp"
4040
"distance.cpp"
41+
"dynamic_nn_search.cpp"
4142
"frame.cpp"
4243
"information.cpp"
4344
"intersection.cpp"
@@ -69,6 +70,7 @@ add_geode_library(
6970
"common.hpp"
7071
"coordinate_system.hpp"
7172
"distance.hpp"
73+
"dynamic_nn_search.hpp"
7274
"frame.hpp"
7375
"information.hpp"
7476
"intersection.hpp"

‎src/geode/geometry/dynamic_nn_search.cpp‎

Lines changed: 176 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,176 @@
1+
/*
2+
* Copyright (c) 2019 - 2024 Geode-solutions
3+
*
4+
* Permission is hereby granted, free of charge, to any person obtaining a copy
5+
* of this software and associated documentation files (the "Software"), to deal
6+
* in the Software without restriction, including without limitation the rights
7+
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8+
* copies of the Software, and to permit persons to whom the Software is
9+
* furnished to do so, subject to the following conditions:
10+
*
11+
* The above copyright notice and this permission notice shall be included in
12+
* all copies or substantial portions of the Software.
13+
*
14+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15+
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16+
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17+
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18+
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19+
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
20+
* SOFTWARE.
21+
*
22+
*/
23+
24+
#include <geode/geometry/dynamic_nn_search.hpp>
25+
26+
#include <numeric>
27+
28+
#include <absl/algorithm/container.h>
29+
30+
#include <async++.h>
31+
32+
#include <nanoflann.hpp>
33+
34+
#include <geode/basic/logger.hpp>
35+
#include <geode/basic/pimpl_impl.hpp>
36+
37+
namespace geode
38+
{
39+
template < index_t dimension >
40+
class DynamicNNSearch< dimension >::Impl
41+
{
42+
public:
43+
explicit Impl( std::vector< Point< dimension > > points )
44+
: cloud_{ std::move( points ) },
45+
dynamic_nn_tree_{ dimension, cloud_ }
46+
{
47+
}
48+
49+
const Point< dimension >& point( const index_t index ) const
50+
{
51+
return cloud_.points.at( index );
52+
}
53+
54+
index_t nb_points() const
55+
{
56+
return cloud_.kdtree_get_point_count();
57+
}
58+
59+
std::vector< index_t > radius_neighbors(
60+
const Point< dimension >& point,
61+
const double threshold_distance ) const
62+
{
63+
std::vector< nanoflann::ResultItem< index_t, double > > results;
64+
nanoflann::RadiusResultSet< double, index_t > resultSet(
65+
threshold_distance * threshold_distance, results );
66+
dynamic_nn_tree_.findNeighbors( resultSet, &copy( point )[0] );
67+
std::sort(
68+
results.begin(), results.end(), nanoflann::IndexDist_Sorter() );
69+
std::vector< index_t > indices;
70+
indices.reserve( results.size() );
71+
for( const auto& result : results )
72+
{
73+
indices.emplace_back( result.first );
74+
}
75+
return indices;
76+
}
77+
78+
void addPoint( const Point< dimension >& point )
79+
{
80+
const auto nb_points = cloud_.kdtree_get_point_count();
81+
cloud_.points.emplace_back( point );
82+
dynamic_nn_tree_.addPoints( nb_points, nb_points );
83+
}
84+
85+
private:
86+
std::array< double, dimension > copy(
87+
const Point< dimension >& point ) const
88+
{
89+
std::array< double, dimension > result;
90+
for( const auto i : LRange{ dimension } )
91+
{
92+
result[i] = point.value( i );
93+
}
94+
return result;
95+
}
96+
struct PointCloud
97+
{
98+
std::vector< Point< dimension > > points;
99+
100+
size_t kdtree_get_point_count() const
101+
{
102+
return points.size();
103+
}
104+
105+
double kdtree_get_pt( size_t idx, size_t dim ) const
106+
{
107+
return points[idx].value( dim );
108+
}
109+
110+
// Optional bounding-box computation: return false to default to a
111+
// standard bbox computation loop.
112+
// Return true if the BBOX was already computed by the class and
113+
// returned in "bb" so it can be avoided to redo it again. Look at
114+
// bb.size() to find out the expected dimensionality (e.g. 2 or 3
115+
// for point clouds)
116+
template < class BBOX >
117+
bool kdtree_get_bbox( BBOX& /* bb */ ) const
118+
{
119+
return false;
120+
}
121+
};
122+
123+
private:
124+
PointCloud cloud_;
125+
nanoflann::KDTreeSingleIndexDynamicAdaptor<
126+
nanoflann::L2_Simple_Adaptor< double, PointCloud >,
127+
PointCloud,
128+
dimension,
129+
index_t >
130+
dynamic_nn_tree_;
131+
};
132+
133+
template < index_t dimension >
134+
DynamicNNSearch< dimension >::DynamicNNSearch(
135+
std::vector< Point< dimension > > points )
136+
: impl_( std::move( points ) )
137+
{
138+
}
139+
140+
template < index_t dimension >
141+
DynamicNNSearch< dimension >::DynamicNNSearch(
142+
DynamicNNSearch&& ) noexcept = default;
143+
144+
template < index_t dimension >
145+
DynamicNNSearch< dimension >::~DynamicNNSearch() = default;
146+
147+
template < index_t dimension >
148+
const Point< dimension >& DynamicNNSearch< dimension >::point(
149+
index_t index ) const
150+
{
151+
return impl_->point( index );
152+
}
153+
154+
template < index_t dimension >
155+
index_t DynamicNNSearch< dimension >::nb_points() const
156+
{
157+
return impl_->nb_points();
158+
}
159+
160+
template < index_t dimension >
161+
std::vector< index_t > DynamicNNSearch< dimension >::radius_neighbors(
162+
const Point< dimension >& point, double threshold_distance ) const
163+
{
164+
return impl_->radius_neighbors( point, threshold_distance );
165+
}
166+
167+
template < index_t dimension >
168+
void DynamicNNSearch< dimension >::addPoint(
169+
const Point< dimension >& point )
170+
{
171+
return impl_->addPoint( point );
172+
}
173+
174+
template class opengeode_geometry_api DynamicNNSearch< 2 >;
175+
template class opengeode_geometry_api DynamicNNSearch< 3 >;
176+
} // namespace geode

‎src/geode/geometry/points_sort.cpp‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ namespace
7070
* the two halves
7171
*/
7272
template < typename Compare >
73-
itr split( const itr& begin, const itr& end, const Compare& cmp )
73+
itr split_container( const itr& begin, const itr& end, const Compare& cmp )
7474
{
7575
if( begin >= end )
7676
{
@@ -108,13 +108,13 @@ namespace
108108

109109
const auto m0 = begin;
110110
const auto m8 = end;
111-
const auto m4 = split( m0, m8, compX );
112-
const auto m2 = split( m0, m4, compY );
113-
const auto m1 = split( m0, m2, compZ );
114-
const auto m3 = split( m2, m4, compZ );
115-
const auto m6 = split( m4, m8, compY );
116-
const auto m5 = split( m4, m6, compZ );
117-
const auto m7 = split( m6, m8, compZ );
111+
const auto m4 = split_container( m0, m8, compX );
112+
const auto m2 = split_container( m0, m4, compY );
113+
const auto m1 = split_container( m0, m2, compZ );
114+
const auto m3 = split_container( m2, m4, compZ );
115+
const auto m6 = split_container( m4, m8, compY );
116+
const auto m5 = split_container( m4, m6, compZ );
117+
const auto m7 = split_container( m6, m8, compZ );
118118
morton_mapping< COORDZ >( points, m0, m1 );
119119
morton_mapping< COORDY >( points, m1, m2 );
120120
morton_mapping< COORDY >( points, m2, m3 );
@@ -142,9 +142,9 @@ namespace
142142

143143
const auto m0 = begin;
144144
const auto m4 = end;
145-
const auto m2 = split( m0, m4, compX );
146-
const auto m1 = split( m0, m2, compY );
147-
const auto m3 = split( m2, m4, compY );
145+
const auto m2 = split_container( m0, m4, compX );
146+
const auto m1 = split_container( m0, m2, compY );
147+
const auto m3 = split_container( m2, m4, compY );
148148
morton_mapping< COORDY >( points, m0, m1 );
149149
morton_mapping< COORDX >( points, m1, m2 );
150150
morton_mapping< COORDX >( points, m2, m3 );

‎tests/geometry/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,12 @@ add_geode_test(
8686
${PROJECT_NAME}::basic
8787
${PROJECT_NAME}::geometry
8888
)
89+
add_geode_test(
90+
SOURCE "test-dynamic-nnsearch.cpp"
91+
DEPENDENCIES
92+
${PROJECT_NAME}::basic
93+
${PROJECT_NAME}::geometry
94+
)
8995
add_geode_test(
9096
SOURCE "test-perpendicular.cpp"
9197
DEPENDENCIES

0 commit comments

Comments
 (0)