corelib
src
rtflann
util
heap.h
Go to the documentation of this file.
1
/***********************************************************************
2
* Software License Agreement (BSD License)
3
*
4
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
5
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
6
*
7
* THE BSD LICENSE
8
*
9
* Redistribution and use in source and binary forms, with or without
10
* modification, are permitted provided that the following conditions
11
* are met:
12
*
13
* 1. Redistributions of source code must retain the above copyright
14
* notice, this list of conditions and the following disclaimer.
15
* 2. Redistributions in binary form must reproduce the above copyright
16
* notice, this list of conditions and the following disclaimer in the
17
* documentation and/or other materials provided with the distribution.
18
*
19
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
20
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
21
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
23
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
24
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
28
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29
*************************************************************************/
30
31
#ifndef RTABMAP_FLANN_HEAP_H_
32
#define RTABMAP_FLANN_HEAP_H_
33
34
#include <algorithm>
35
#include <vector>
36
#include <
rtabmap/utilite/ULogger.h
>
37
38
namespace
rtflann
39
{
40
48
template
<
typename
T>
49
class
Heap
50
{
51
56
std::vector<T>
heap
;
57
int
length
;
58
62
int
count
;
63
64
65
66
public
:
74
Heap
(
int
size
)
75
{
76
length
=
size
;
77
heap
.reserve(
length
);
78
count
= 0;
79
}
80
85
int
size
()
86
{
87
return
count
;
88
}
89
94
int
capacity
()
95
{
96
return
length
;
97
}
98
104
bool
empty
()
105
{
106
return
size
()==0;
107
}
108
112
void
clear
()
113
{
114
heap
.clear();
115
count
= 0;
116
}
117
118
struct
CompareT
119
{
120
using
result_type
= bool;
121
using
first_argument_type
=
T
;
122
using
second_argument_type
=
T
;
123
bool
operator()
(
const
T
& t_1,
const
T
& t_2)
const
124
{
125
return
t_2 < t_1;
126
}
127
};
128
138
void
insert
(
const
T
& value)
139
{
140
/* If heap is full, then return without adding this element. */
141
if
(
count
==
length
) {
142
return
;
143
}
144
145
UASSERT
(
heap
.size() <
heap
.capacity());
146
heap
.push_back(value);
147
static
CompareT compareT;
148
std::push_heap(
heap
.begin(),
heap
.end(), compareT);
149
++
count
;
150
}
151
152
153
161
bool
popMin
(
T
& value)
162
{
163
if
(
count
== 0) {
164
return
false
;
165
}
166
167
value
=
heap
[0];
168
static
CompareT
compareT;
169
std::pop_heap(
heap
.begin(),
heap
.end(), compareT);
170
heap
.pop_back();
171
--
count
;
172
173
return
true
;
/* Return old last node. */
174
}
175
};
176
177
178
template
<
typename
T>
179
class
IntervalHeap
180
{
181
struct
Interval
182
{
183
T
left
;
184
T
right
;
185
};
186
191
std::vector<Interval>
heap
;
192
size_t
capacity_
;
193
size_t
size_
;
194
195
public
:
203
IntervalHeap
(
int
capacity) :
capacity_
(capacity),
size_
(0)
204
{
205
heap
.resize(capacity/2 + capacity%2 + 1);
// 1-based indexing
206
}
207
211
size_t
size
()
212
{
213
return
size_
;
214
}
215
220
bool
empty
()
221
{
222
return
size_
==0;
223
}
224
228
void
clear
()
229
{
230
size_
= 0;
231
}
232
233
void
insert
(
const
T
& value)
234
{
235
/* If heap is full, then return without adding this element. */
236
if
(
size_
==
capacity_
) {
237
return
;
238
}
239
240
// insert into the root
241
if
(
size_
<2) {
242
if
(
size_
==0) {
243
heap
[1].left =
value
;
244
heap
[1].right =
value
;
245
}
246
else
{
247
if
(
value
<
heap
[1].left) {
248
heap
[1].left =
value
;
249
}
250
else
{
251
heap
[1].right =
value
;
252
}
253
}
254
++
size_
;
255
return
;
256
}
257
258
size_t
last_pos =
size_
/2 +
size_
%2;
259
bool
min_heap;
260
261
if
(
size_
%2) {
// odd number of elements
262
min_heap = (
value
<
heap
[last_pos].left)?
true
:
false
;
263
}
264
else
{
265
++last_pos;
266
min_heap = (
value
<
heap
[last_pos/2].left)?
true
:
false
;
267
}
268
269
if
(min_heap) {
270
size_t
pos
= last_pos;
271
size_t
par =
pos
/2;
272
while
(
pos
>1 && value <
heap
[par].left) {
273
heap
[
pos
].left =
heap
[par].left;
274
pos
= par;
275
par =
pos
/2;
276
}
277
heap
[
pos
].left =
value
;
278
++
size_
;
279
280
if
(
size_
%2) {
// duplicate element in last position if size is odd
281
heap
[last_pos].right =
heap
[last_pos].left;
282
}
283
}
284
else
{
285
size_t
pos
= last_pos;
286
size_t
par =
pos
/2;
287
while
(
pos
>1 &&
heap
[par].right < value) {
288
heap
[
pos
].right =
heap
[par].right;
289
pos
= par;
290
par =
pos
/2;
291
}
292
heap
[
pos
].right =
value
;
293
++
size_
;
294
295
if
(
size_
%2) {
// duplicate element in last position if size is odd
296
heap
[last_pos].left =
heap
[last_pos].right;
297
}
298
}
299
}
300
301
307
bool
popMin
(
T
& value)
308
{
309
if
(
size_
== 0) {
310
return
false
;
311
}
312
313
value
=
heap
[1].left;
314
size_t
last_pos =
size_
/2 +
size_
%2;
315
T
elem =
heap
[last_pos].left;
316
317
if
(
size_
% 2) {
// odd number of elements
318
--last_pos;
319
}
320
else
{
321
heap
[last_pos].left =
heap
[last_pos].right;
322
}
323
--
size_
;
324
if
(
size_
<2)
return
true
;
325
326
size_t
crt=1;
// root node
327
size_t
child = crt*2;
328
329
while
(child <= last_pos) {
330
if
(child < last_pos &&
heap
[child+1].left <
heap
[child].left) ++child;
// pick the child with min
331
332
if
(!(
heap
[child].left<elem))
break
;
333
334
heap
[crt].left =
heap
[child].left;
335
if
(
heap
[child].right<elem) {
336
std::swap
(elem,
heap
[child].right);
337
}
338
339
crt = child;
340
child *= 2;
341
}
342
heap
[crt].left = elem;
343
return
true
;
344
}
345
346
352
bool
popMax
(
T
& value)
353
{
354
if
(
size_
== 0) {
355
return
false
;
356
}
357
358
value
=
heap
[1].right;
359
size_t
last_pos =
size_
/2 +
size_
%2;
360
T
elem =
heap
[last_pos].right;
361
362
if
(
size_
%2) {
// odd number of elements
363
--last_pos;
364
}
365
else
{
366
heap
[last_pos].right =
heap
[last_pos].left;
367
}
368
--
size_
;
369
if
(
size_
<2)
return
true
;
370
371
size_t
crt=1;
// root node
372
size_t
child = crt*2;
373
374
while
(child <= last_pos) {
375
if
(child < last_pos &&
heap
[child].right <
heap
[child+1].right) ++child;
// pick the child with max
376
377
if
(!(elem <
heap
[child].right))
break
;
378
379
heap
[crt].right =
heap
[child].right;
380
if
(elem<
heap
[child].left) {
381
std::swap
(elem,
heap
[child].left);
382
}
383
384
crt = child;
385
child *= 2;
386
}
387
heap
[crt].right = elem;
388
return
true
;
389
}
390
391
392
bool
getMin
(
T
& value)
393
{
394
if
(
size_
==0) {
395
return
false
;
396
}
397
value
=
heap
[1].left;
398
return
true
;
399
}
400
401
402
bool
getMax
(
T
& value)
403
{
404
if
(
size_
==0) {
405
return
false
;
406
}
407
value
=
heap
[1].right;
408
return
true
;
409
}
410
};
411
412
413
template
<
typename
T>
414
class
BoundedHeap
415
{
416
IntervalHeap<T>
interval_heap_
;
417
size_t
capacity_
;
418
public
:
419
BoundedHeap
(
size_t
capacity) :
interval_heap_
(capacity),
capacity_
(capacity)
420
{
421
422
}
423
427
int
size
()
428
{
429
return
interval_heap_
.size();
430
}
431
436
bool
empty
()
437
{
438
return
interval_heap_
.empty();
439
}
440
444
void
clear
()
445
{
446
interval_heap_
.clear();
447
}
448
449
void
insert
(
const
T
& value)
450
{
451
if
(
interval_heap_
.size()==
capacity_
) {
452
T
max
;
453
interval_heap_
.getMax(
max
);
454
if
(
max
<
value
)
return
;
455
interval_heap_
.popMax(
max
);
456
}
457
interval_heap_
.insert(value);
458
}
459
460
bool
popMin
(
T
& value)
461
{
462
return
interval_heap_
.popMin(
value
);
463
}
464
};
465
466
467
468
}
469
470
#endif //FLANN_HEAP_H_
rtflann::BoundedHeap::BoundedHeap
BoundedHeap(size_t capacity)
Definition:
heap.h:447
rtflann::Heap::insert
void insert(const T &value)
Definition:
heap.h:194
rtflann::Heap::CompareT::first_argument_type
T first_argument_type
Definition:
heap.h:177
rtflann::IntervalHeap::popMax
bool popMax(T &value)
Definition:
heap.h:380
rtflann::BoundedHeap::popMin
bool popMin(T &value)
Definition:
heap.h:488
rtflann::Heap::heap
std::vector< T > heap
Definition:
heap.h:112
rtflann::IntervalHeap::insert
void insert(const T &value)
Definition:
heap.h:261
rtflann::Heap::popMin
bool popMin(T &value)
Definition:
heap.h:217
rtflann::Heap::CompareT::second_argument_type
T second_argument_type
Definition:
heap.h:178
rtflann::Heap::empty
bool empty()
Definition:
heap.h:160
rtflann::IntervalHeap::getMin
bool getMin(T &value)
Definition:
heap.h:420
rtflann::BoundedHeap::empty
bool empty()
Definition:
heap.h:464
rtflann::BoundedHeap::size
int size()
Definition:
heap.h:455
rtflann::IntervalHeap::size_
size_t size_
Definition:
heap.h:221
rtflann::BoundedHeap::insert
void insert(const T &value)
Definition:
heap.h:477
rtflann::Heap::capacity
int capacity()
Definition:
heap.h:150
rtflann::IntervalHeap::capacity_
size_t capacity_
Definition:
heap.h:220
rtflann::Heap::count
int count
Definition:
heap.h:118
rtflann::IntervalHeap::clear
void clear()
Definition:
heap.h:256
rtflann::BoundedHeap::interval_heap_
IntervalHeap< T > interval_heap_
Definition:
heap.h:444
rtflann::IntervalHeap
Definition:
heap.h:207
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtflann::Heap::CompareT::operator()
bool operator()(const T &t_1, const T &t_2) const
Definition:
heap.h:179
rtflann::IntervalHeap::heap
std::vector< Interval > heap
Definition:
heap.h:219
UASSERT
#define UASSERT(condition)
rtflann::Heap::length
int length
Definition:
heap.h:113
Eigen::Triplet< double >
rtflann::Heap::size
int size()
Definition:
heap.h:141
rtflann::IntervalHeap::Interval::left
T left
Definition:
heap.h:211
ULogger.h
ULogger class and convenient macros.
rtflann::Heap::CompareT::result_type
bool result_type
Definition:
heap.h:176
rtflann::IntervalHeap::IntervalHeap
IntervalHeap(int capacity)
Definition:
heap.h:231
rtflann::BoundedHeap::clear
void clear()
Definition:
heap.h:472
rtflann::IntervalHeap::empty
bool empty()
Definition:
heap.h:248
rtflann::IntervalHeap::size
size_t size()
Definition:
heap.h:239
rtflann::BoundedHeap::capacity_
size_t capacity_
Definition:
heap.h:445
rtflann::IntervalHeap::getMax
bool getMax(T &value)
Definition:
heap.h:430
rtflann::IntervalHeap::Interval::right
T right
Definition:
heap.h:212
rtflann::Heap::clear
void clear()
Definition:
heap.h:168
pos
rtflann::Heap::CompareT
Definition:
heap.h:174
rtflann::IntervalHeap::popMin
bool popMin(T &value)
Definition:
heap.h:335
rtflann
Definition:
all_indices.h:49
value
value
rtflann::Heap::Heap
Heap(int size)
Definition:
heap.h:130
rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11