31 #ifndef RTABMAP_FLANN_HEAP_H_ 32 #define RTABMAP_FLANN_HEAP_H_ 118 struct CompareT :
public std::binary_function<T,T,bool>
138 if (count == length) {
142 UASSERT(heap.size() < heap.capacity());
143 heap.push_back(value);
145 std::push_heap(heap.begin(), heap.end(), compareT);
166 std::pop_heap(heap.begin(), heap.end(), compareT);
175 template <
typename T>
202 heap.resize(capacity/2 + capacity%2 + 1);
233 if (size_ == capacity_) {
240 heap[1].left = value;
241 heap[1].right = value;
244 if (value<heap[1].left) {
245 heap[1].left = value;
248 heap[1].right = value;
255 size_t last_pos = size_/2 + size_%2;
259 min_heap = (value<heap[last_pos].left)?
true :
false;
263 min_heap = (value<heap[last_pos/2].left)?
true :
false;
267 size_t pos = last_pos;
269 while (pos>1 && value < heap[par].left) {
270 heap[pos].left = heap[par].left;
274 heap[pos].left = value;
278 heap[last_pos].right = heap[last_pos].left;
282 size_t pos = last_pos;
284 while (pos>1 && heap[par].right < value) {
285 heap[pos].right = heap[par].right;
289 heap[pos].right = value;
293 heap[last_pos].left = heap[last_pos].right;
310 value = heap[1].left;
311 size_t last_pos = size_/2 + size_%2;
312 T elem = heap[last_pos].left;
318 heap[last_pos].left = heap[last_pos].right;
321 if (size_<2)
return true;
324 size_t child = crt*2;
326 while (child <= last_pos) {
327 if (child < last_pos && heap[child+1].left < heap[child].left) ++child;
329 if (!(heap[child].left<elem))
break;
331 heap[crt].left = heap[child].left;
332 if (heap[child].right<elem) {
333 std::swap(elem, heap[child].right);
339 heap[crt].left = elem;
355 value = heap[1].right;
356 size_t last_pos = size_/2 + size_%2;
357 T elem = heap[last_pos].right;
363 heap[last_pos].right = heap[last_pos].left;
366 if (size_<2)
return true;
369 size_t child = crt*2;
371 while (child <= last_pos) {
372 if (child < last_pos && heap[child].right < heap[child+1].right) ++child;
374 if (!(elem < heap[child].right))
break;
376 heap[crt].right = heap[child].right;
377 if (elem<heap[child].left) {
378 std::swap(elem, heap[child].left);
384 heap[crt].right = elem;
394 value = heap[1].left;
404 value = heap[1].right;
410 template <
typename T>
426 return interval_heap_.
size();
435 return interval_heap_.
empty();
443 interval_heap_.
clear();
448 if (interval_heap_.
size()==capacity_) {
450 interval_heap_.
getMax(max);
451 if (max<value)
return;
452 interval_heap_.
popMax(max);
454 interval_heap_.
insert(value);
459 return interval_heap_.
popMin(value);
467 #endif //FLANN_HEAP_H_
void insert(const T &value)
void insert(const T &value)
IntervalHeap< T > interval_heap_
BoundedHeap(size_t capacity)
#define UASSERT(condition)
std::vector< Interval > heap
bool operator()(const T &t_1, const T &t_2) const
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
ULogger class and convenient macros.
IntervalHeap(int capacity)
void insert(const T &value)