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
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>
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;
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