Branch data Line data Source code
1 : : /*
2 : : * Copyright 2010 Savarese Software Research Corporation
3 : : *
4 : : * Licensed under the Apache License, Version 2.0 (the "License");
5 : : * you may not use this file except in compliance with the License.
6 : : * You may obtain a copy of the License at
7 : : *
8 : : * https://www.savarese.com/software/ApacheLicense-2.0
9 : : *
10 : : * Unless required by applicable law or agreed to in writing, software
11 : : * distributed under the License is distributed on an "AS IS" BASIS,
12 : : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : : * See the License for the specific language governing permissions and
14 : : * limitations under the License.
15 : : */
16 : :
17 : : #ifndef __SSRC_SPATIAL_DETAIL_KDTREE_NEAREST_NEIGHBORS_H
18 : : #define __SSRC_SPATIAL_DETAIL_KDTREE_NEAREST_NEIGHBORS_H
19 : :
20 : : #include <ssrc/spatial/detail/coordinate_limits.h>
21 : : #include <ssrc/spatial/distance.h>
22 : :
23 : : #include <vector>
24 : : #include <algorithm>
25 : :
26 : : #include <boost/shared_ptr.hpp>
27 : : #include <boost/shared_container_iterator.hpp>
28 : : #include <boost/iterator/transform_iterator.hpp>
29 : :
30 : : __BEGIN_NS_SSRC_SPATIAL
31 : :
32 : : namespace detail {
33 : :
34 : : template<typename TreeTraits, typename Distance>
35 : 988 : struct kd_tree_nn_pq_value {
36 : : typedef TreeTraits traits;
37 : : typedef Distance distance_type;
38 : : typedef typename traits::node_type node_type;
39 : :
40 : : distance_type distance;
41 : : node_type *node;
42 : :
43 : 1342 : kd_tree_nn_pq_value(const distance_type distance = distance_type(0),
44 : : node_type * const node = 0) :
45 : 1342 : distance(distance), node(node)
46 : 1342 : { }
47 : :
48 : 4093 : friend bool operator<(const kd_tree_nn_pq_value & v1,
49 : : const kd_tree_nn_pq_value & v2)
50 : : {
51 : 4093 : return (v1.distance < v2.distance);
52 : : }
53 : : };
54 : :
55 : : template<typename TreeTraits, typename Distance>
56 : : class kd_tree_nearest_neighbors {
57 : : typedef TreeTraits traits;
58 : : typedef typename traits::key_type key_type;
59 : : typedef typename traits::node_type node_type;
60 : : typedef typename traits::discriminator_type discriminator_type;
61 : : typedef Distance distance_type;
62 : : typedef kd_tree_nn_pq_value<traits, distance_type> pq_value_type;
63 : : typedef std::vector<pq_value_type> pq_type;
64 : : typedef boost::shared_container_iterator<pq_type> shared_iterator;
65 : :
66 : : mutable bool _omit_query_point;
67 : : mutable unsigned int _num_neighbors;
68 : : mutable distance_type _min_distance;
69 : : mutable pq_type *_pq;
70 : : mutable key_type _query;
71 : :
72 : : struct get_node_pair {
73 : : typedef
74 : : typename kd_tree_nearest_neighbors::traits::const_reference result_type;
75 : : typedef kd_tree_nearest_neighbors::pq_value_type pq_value_type;
76 : :
77 : 350 : result_type operator()(const pq_value_type & v) const {
78 : 350 : return v.node->pair();
79 : : }
80 : : };
81 : :
82 : 1342 : void push(const pq_value_type & v) const {
83 : 1342 : _pq->push_back(v);
84 : 1342 : std::push_heap(_pq->begin(), _pq->end());
85 : 1342 : }
86 : :
87 : 988 : void pop() const {
88 : 988 : std::pop_heap(_pq->begin(), _pq->end());
89 : 988 : _pq->pop_back();
90 : 988 : }
91 : :
92 : 1070 : const pq_value_type & top() const {
93 : 1070 : return _pq->front();
94 : : }
95 : :
96 : 3836 : void find(node_type * const node) const {
97 [ + + ][ + + ]: 3836 : if(node == 0)
[ + + ]
98 : 3836 : return;
99 : :
100 : 2628 : const discriminator_type discriminator = node->discriminator;
101 : 2628 : const key_type & point = node->point();
102 : 2628 : distance_type d2 = euclidean_distance<key_type>::d2(_query, point);
103 : :
104 [ + + ][ + + ]: 2628 : if(d2 < _min_distance && (d2 != 0 || !_omit_query_point)) {
[ + + + + ]
[ + + ]
[ + + + + ]
[ + + ][ + + ]
105 [ + + ][ + + ]: 1342 : if(_pq->size() == _num_neighbors) {
[ + + ]
106 : 988 : pop();
107 : 988 : push(pq_value_type(d2, node));
108 : 988 : _min_distance = top().distance;
109 : : } else {
110 : 354 : push(pq_value_type(d2, node));
111 [ + + + + : 354 : if(_pq->size() == _num_neighbors) {
+ + ]
112 : 82 : _min_distance = top().distance;
113 : : }
114 : : }
115 : : }
116 : :
117 : : // Distance to partition plane.
118 : : const distance_type dp =
119 : : static_cast<distance_type>(_query[discriminator]) -
120 : 2628 : static_cast<distance_type>(point[discriminator]);
121 : :
122 : 2628 : d2 = dp*dp;
123 : :
124 [ + + + + : 2628 : if(dp < 0) {
+ + ]
125 : 708 : find(node->child_low);
126 [ + + + + : 708 : if(d2 < _min_distance) {
+ + ]
127 : 158 : find(node->child_high);
128 : : }
129 : : } else {
130 : 1920 : find(node->child_high);
131 [ + + + + : 1920 : if(d2 < _min_distance) {
+ + ]
132 : 968 : find(node->child_low);
133 : : }
134 : : }
135 : : }
136 : :
137 : : public:
138 : : typedef
139 : : boost::transform_iterator<get_node_pair, shared_iterator>
140 : : iterator;
141 : :
142 : 82 : kd_tree_nearest_neighbors() { }
143 : :
144 : 82 : std::pair<iterator,iterator> find(node_type * const root,
145 : : const key_type & query,
146 : : const unsigned int num_neighbors,
147 : : const bool omit_query_point) const
148 : : {
149 [ + - ][ + - ]: 164 : boost::shared_ptr<pq_type> neighbors(new pq_type());
[ + - ]
150 : 82 : _pq = neighbors.get();
151 [ + - + - : 82 : _pq->reserve(num_neighbors);
+ - ]
152 : :
153 : 82 : _omit_query_point = omit_query_point;
154 : 82 : _num_neighbors = num_neighbors;
155 : 82 : _min_distance = coordinate_limits<distance_type>::highest();
156 : 82 : _query = query;
157 : :
158 [ + - + - : 82 : if(num_neighbors > 0) {
+ - ]
159 [ + - ][ + - ]: 82 : find(root);
[ + - ]
160 : : }
161 : :
162 [ + - ][ + - ]: 82 : std::sort_heap(_pq->begin(), _pq->end());
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ]
163 : 82 : _pq = 0;
164 : :
165 : :
166 : : return
167 : : std::pair<iterator,iterator>(
168 : : iterator(shared_iterator(neighbors->begin(), neighbors),
169 : : get_node_pair()),
170 : : iterator(shared_iterator(neighbors->end(), neighbors),
171 [ + - ][ + - ]: 164 : get_node_pair()));
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ][ + - ]
[ + - ]
172 : :
173 : : }
174 : : };
175 : :
176 : : }
177 : :
178 : : __END_NS_SSRC_SPATIAL
179 : :
180 : : #endif
|
