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>
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 : public std::binary_function<T,T,bool>
119  {
120  bool operator()(const T& t_1, const T& t_2) const
121  {
122  return t_2 < t_1;
123  }
124  };
125 
135  void insert(const T& value)
136  {
137  /* If heap is full, then return without adding this element. */
138  if (count == length) {
139  return;
140  }
141 
142  UASSERT(heap.size() < heap.capacity());
143  heap.push_back(value);
144  static CompareT compareT;
145  std::push_heap(heap.begin(), heap.end(), compareT);
146  ++count;
147  }
148 
149 
150 
158  bool popMin(T& value)
159  {
160  if (count == 0) {
161  return false;
162  }
163 
164  value = heap[0];
165  static CompareT compareT;
166  std::pop_heap(heap.begin(), heap.end(), compareT);
167  heap.pop_back();
168  --count;
169 
170  return true; /* Return old last node. */
171  }
172 };
173 
174 
175 template <typename T>
177 {
178  struct Interval
179  {
180  T left;
181  T right;
182  };
183 
188  std::vector<Interval> heap;
189  size_t capacity_;
190  size_t size_;
191 
192 public:
200  IntervalHeap(int capacity) : capacity_(capacity), size_(0)
201  {
202  heap.resize(capacity/2 + capacity%2 + 1); // 1-based indexing
203  }
204 
208  size_t size()
209  {
210  return size_;
211  }
212 
217  bool empty()
218  {
219  return size_==0;
220  }
221 
225  void clear()
226  {
227  size_ = 0;
228  }
229 
230  void insert(const T& value)
231  {
232  /* If heap is full, then return without adding this element. */
233  if (size_ == capacity_) {
234  return;
235  }
236 
237  // insert into the root
238  if (size_<2) {
239  if (size_==0) {
240  heap[1].left = value;
241  heap[1].right = value;
242  }
243  else {
244  if (value<heap[1].left) {
245  heap[1].left = value;
246  }
247  else {
248  heap[1].right = value;
249  }
250  }
251  ++size_;
252  return;
253  }
254 
255  size_t last_pos = size_/2 + size_%2;
256  bool min_heap;
257 
258  if (size_%2) { // odd number of elements
259  min_heap = (value<heap[last_pos].left)? true : false;
260  }
261  else {
262  ++last_pos;
263  min_heap = (value<heap[last_pos/2].left)? true : false;
264  }
265 
266  if (min_heap) {
267  size_t pos = last_pos;
268  size_t par = pos/2;
269  while (pos>1 && value < heap[par].left) {
270  heap[pos].left = heap[par].left;
271  pos = par;
272  par = pos/2;
273  }
274  heap[pos].left = value;
275  ++size_;
276 
277  if (size_%2) { // duplicate element in last position if size is odd
278  heap[last_pos].right = heap[last_pos].left;
279  }
280  }
281  else {
282  size_t pos = last_pos;
283  size_t par = pos/2;
284  while (pos>1 && heap[par].right < value) {
285  heap[pos].right = heap[par].right;
286  pos = par;
287  par = pos/2;
288  }
289  heap[pos].right = value;
290  ++size_;
291 
292  if (size_%2) { // duplicate element in last position if size is odd
293  heap[last_pos].left = heap[last_pos].right;
294  }
295  }
296  }
297 
298 
304  bool popMin(T& value)
305  {
306  if (size_ == 0) {
307  return false;
308  }
309 
310  value = heap[1].left;
311  size_t last_pos = size_/2 + size_%2;
312  T elem = heap[last_pos].left;
313 
314  if (size_ % 2) { // odd number of elements
315  --last_pos;
316  }
317  else {
318  heap[last_pos].left = heap[last_pos].right;
319  }
320  --size_;
321  if (size_<2) return true;
322 
323  size_t crt=1; // root node
324  size_t child = crt*2;
325 
326  while (child <= last_pos) {
327  if (child < last_pos && heap[child+1].left < heap[child].left) ++child; // pick the child with min
328 
329  if (!(heap[child].left<elem)) break;
330 
331  heap[crt].left = heap[child].left;
332  if (heap[child].right<elem) {
333  std::swap(elem, heap[child].right);
334  }
335 
336  crt = child;
337  child *= 2;
338  }
339  heap[crt].left = elem;
340  return true;
341  }
342 
343 
349  bool popMax(T& value)
350  {
351  if (size_ == 0) {
352  return false;
353  }
354 
355  value = heap[1].right;
356  size_t last_pos = size_/2 + size_%2;
357  T elem = heap[last_pos].right;
358 
359  if (size_%2) { // odd number of elements
360  --last_pos;
361  }
362  else {
363  heap[last_pos].right = heap[last_pos].left;
364  }
365  --size_;
366  if (size_<2) return true;
367 
368  size_t crt=1; // root node
369  size_t child = crt*2;
370 
371  while (child <= last_pos) {
372  if (child < last_pos && heap[child].right < heap[child+1].right) ++child; // pick the child with max
373 
374  if (!(elem < heap[child].right)) break;
375 
376  heap[crt].right = heap[child].right;
377  if (elem<heap[child].left) {
378  std::swap(elem, heap[child].left);
379  }
380 
381  crt = child;
382  child *= 2;
383  }
384  heap[crt].right = elem;
385  return true;
386  }
387 
388 
389  bool getMin(T& value)
390  {
391  if (size_==0) {
392  return false;
393  }
394  value = heap[1].left;
395  return true;
396  }
397 
398 
399  bool getMax(T& value)
400  {
401  if (size_==0) {
402  return false;
403  }
404  value = heap[1].right;
405  return true;
406  }
407 };
408 
409 
410 template <typename T>
412 {
414  size_t capacity_;
415 public:
416  BoundedHeap(size_t capacity) : interval_heap_(capacity), capacity_(capacity)
417  {
418 
419  }
420 
424  int size()
425  {
426  return interval_heap_.size();
427  }
428 
433  bool empty()
434  {
435  return interval_heap_.empty();
436  }
437 
441  void clear()
442  {
443  interval_heap_.clear();
444  }
445 
446  void insert(const T& value)
447  {
448  if (interval_heap_.size()==capacity_) {
449  T max;
450  interval_heap_.getMax(max);
451  if (max<value) return;
452  interval_heap_.popMax(max);
453  }
454  interval_heap_.insert(value);
455  }
456 
457  bool popMin(T& value)
458  {
459  return interval_heap_.popMin(value);
460  }
461 };
462 
463 
464 
465 }
466 
467 #endif //FLANN_HEAP_H_
bool popMin(T &value)
Definition: heap.h:304
int count
Definition: heap.h:62
bool getMin(T &value)
Definition: heap.h:389
int capacity()
Definition: heap.h:94
int length
Definition: heap.h:57
int size()
Definition: heap.h:85
std::vector< T > heap
Definition: heap.h:56
void insert(const T &value)
Definition: heap.h:230
void insert(const T &value)
Definition: heap.h:135
size_t capacity_
Definition: heap.h:189
IntervalHeap< T > interval_heap_
Definition: heap.h:413
BoundedHeap(size_t capacity)
Definition: heap.h:416
#define UASSERT(condition)
bool empty()
Definition: heap.h:104
std::vector< Interval > heap
Definition: heap.h:188
bool popMin(T &value)
Definition: heap.h:158
size_t capacity_
Definition: heap.h:414
bool popMin(T &value)
Definition: heap.h:457
bool operator()(const T &t_1, const T &t_2) const
Definition: heap.h:120
bool getMax(T &value)
Definition: heap.h:399
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
void clear()
Definition: heap.h:112
ULogger class and convenient macros.
Heap(int size)
Definition: heap.h:74
IntervalHeap(int capacity)
Definition: heap.h:200
bool popMax(T &value)
Definition: heap.h:349
void insert(const T &value)
Definition: heap.h:446
size_t size()
Definition: heap.h:208


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59