libnabo  1.0.4
index_heap.h
Go to the documentation of this file.
1 /*
2 
3 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
4 You can contact the author at <stephane at magnenat dot net>
5 
6 All rights reserved.
7 
8 Redistribution and use in source and binary forms, with or without
9 modification, are permitted provided that the following conditions are met:
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the <organization> nor the
16  names of its contributors may be used to endorse or promote products
17  derived from this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 
30 */
31 
32 #ifndef __INDEX_HEAP_H
33 #define __INDEX_HEAP_H
34 
35 #include "nabo.h"
36 #include <vector>
37 #include <algorithm>
38 #include <limits>
39 #include <iostream>
40 
46 namespace Nabo
47 {
49 
51  template<typename IT, typename VT>
52  struct IndexHeapSTL
53  {
55  typedef IT Index;
57  typedef VT Value;
58 
60  struct Entry
61  {
62  IT index;
63  VT value;
64 
66  Entry(const IT index, const VT value): index(index), value(value) {}
68  friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
69  };
71  typedef std::vector<Entry> Entries;
73  typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
75  typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
76 
80  const size_t nbNeighbours;
81 
82 
84 
85  IndexHeapSTL(const size_t size):
86  data(1, Entry(0, std::numeric_limits<VT>::infinity())),
87  nbNeighbours(size)
88  {
89  data.reserve(size);
90  }
91 
93  inline void reset()
94  {
95  data.clear();
96  data.push_back(Entry(0, std::numeric_limits<VT>::infinity()));
97  }
98 
100 
101  inline const VT& headValue() const { return data.front().value; }
102 
104 
106  inline void replaceHead(const Index index, const Value value)
107  {
108 
109  if (data.size() == nbNeighbours)
110  { // we have enough neighbours to discard largest
111  pop_heap(data.begin(), data.end());
112  data.back() = Entry(index, value);
113  }
114  else
115  { // missing neighbours
116  data.push_back(Entry(index, value));
117  }
118  // ensure heap
119  push_heap(data.begin(), data.end());
120  }
121 
123  inline void sort()
124  {
125  sort_heap (data.begin(), data.end());
126  }
127 
129 
131  template<typename DI, typename DV>
132  inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
133  {
134  // note: we must implement this hack because of problem with reference to temporary
135  // C++0x will solve this with rvalue
136  // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
137  // for more informations
138  size_t i = 0;
139  for (; i < data.size(); ++i)
140  {
141  const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
142  const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
143  }
144  for (; i < nbNeighbours; ++i)
145  {
146  const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = 0;
147  const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = std::numeric_limits<VT>::infinity();
148  }
149  }
150 
151 #if 0
152 
154  inline IndexVector getIndexes() const
155  {
156  IndexVector indexes(data.capacity());
157  size_t i = 0;
158  for (; i < data.size(); ++i)
159  indexes.coeffRef(i) = data[i].index;
160  for (; i < data.capacity(); ++i)
161  indexes.coeffRef(i) = 0;
162  return indexes;
163  }
164 #endif
165  };
166 
167 #if 0
168 
171  template<typename IT, typename VT>
172  struct IndexHeapBruteForceVector
173  {
175  typedef IT Index;
177  typedef VT Value;
178 
180  struct Entry
181  {
182  IT index;
183  VT value;
184 
186  Entry(const IT index, const VT value): index(index), value(value) {}
188  friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
189  };
191  typedef std::vector<Entry> Entries;
193  typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
194 
196  Entries data;
198  const VT& headValueRef;
200  const size_t sizeMinusOne;
201 
203 
204  IndexHeapBruteForceVector(const size_t size):
205  data(size, Entry(0, std::numeric_limits<VT>::infinity())),
206  headValueRef((data.end() - 1)->value),
207  sizeMinusOne(data.size() - 1)
208  {
209  }
210 
212  inline void reset()
213  {
214  for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
215  it->value = std::numeric_limits<VT>::infinity();
216  }
217 
219 
220  inline const VT& headValue() const { return data[0].value; }
221 
223 
225  inline void replaceHead(const Index index, const Value value)
226  {
227  register size_t i = 0;
228  for (; i < sizeMinusOne; ++i)
229  {
230  if (data[i + 1].value > value)
231  data[i] = data[i + 1];
232  else
233  break;
234  }
235  data[i].value = value;
236  data[i].index = index;
237  }
238 
240  inline void sort()
241  {
242  // no need to sort as data are already sorted
243  }
244 
246 
247  inline IndexVector getIndexes() const
248  {
249  IndexVector indexes(data.size());
250  for (size_t i = 0; i < data.size(); ++i)
251  indexes.coeffRef(i) = data[sizeMinusOne-i].index;
252  return indexes;
253  }
254  };
255 #endif
256 
258 
260  template<typename IT, typename VT>
262  {
264  typedef IT Index;
266  typedef VT Value;
267 
269  struct Entry
270  {
271  IT index;
272  VT value;
273 
275  Entry(const IT index, const VT value): index(index), value(value) {}
277  friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
278  };
280  typedef std::vector<Entry> Entries;
282  typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
284  typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
285 
289  const VT& headValueRef;
291  const size_t sizeMinusOne;
292 
294 
295  IndexHeapBruteForceVector(const size_t size):
296  data(size, Entry(0, std::numeric_limits<VT>::infinity())),
297  headValueRef((data.end() - 1)->value),
298  sizeMinusOne(data.size() - 1)
299  {
300  }
301 
303  inline void reset()
304  {
305  for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
306  it->value = std::numeric_limits<VT>::infinity();
307  }
308 
310 
311  inline const VT& headValue() const { return headValueRef; }
312 
314 
316  inline void replaceHead(const Index index, const Value value)
317  {
318  register size_t i;
319  for (i = sizeMinusOne; i > 0; --i)
320  {
321  if (data[i-1].value > value)
322  data[i] = data[i-1];
323  else
324  break;
325  }
326  data[i].value = value;
327  data[i].index = index;
328  }
329 
331  inline void sort()
332  {
333  // no need to sort as data are already sorted
334  }
335 
337 
339  template<typename DI, typename DV>
340  inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
341  {
342  // note: we must implement this hack because of problem with reference to temporary
343  // C++0x will solve this with rvalue
344  // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
345  // for more informations
346  for (size_t i = 0; i < data.size(); ++i)
347  {
348  const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
349  const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
350  }
351  }
352 #if 0
353 
355  inline IndexVector getIndexes() const
356  {
357  IndexVector indexes(data.size());
358  for (size_t i = 0; i < data.size(); ++i)
359  indexes.coeffRef(i) = data[i].index;
360  return indexes;
361  }
362 #endif
363  };
364 }
365 
366 #endif // __INDEX_HEAP_H
IndexHeapBruteForceVector(const size_t size)
Constructor.
Definition: index_heap.h:295
friend bool operator<(const Entry &e0, const Entry &e1)
return true if e0 is smaller than e1, false otherwise
Definition: index_heap.h:277
void replaceHead(const Index index, const Value value)
replace the largest value of the heap
Definition: index_heap.h:316
void sort()
sort the entries, from the smallest to the largest
Definition: index_heap.h:331
const VT & headValue() const
get the largest value of the heap
Definition: index_heap.h:311
void sort()
sort the entries, from the smallest to the largest
Definition: index_heap.h:123
an entry of the heap vector
Definition: index_heap.h:269
Eigen::Matrix< Value, Eigen::Dynamic, 1 > ValueVector
vector of values
Definition: index_heap.h:75
IT index
index of point
Definition: index_heap.h:62
std::vector< Entry > Entries
vector of entry, type for the storage of the tree
Definition: index_heap.h:71
const VT & headValueRef
reference to the largest value in the tree, to optimise access speed
Definition: index_heap.h:289
VT value
distance for this point
Definition: index_heap.h:272
Eigen::Matrix< Index, Eigen::Dynamic, 1 > IndexVector
vector of indices
Definition: index_heap.h:282
std::vector< Entry > Entries
vector of entry, type for the storage of the tree
Definition: index_heap.h:280
Entries data
storage for the tree
Definition: index_heap.h:287
Entry(const IT index, const VT value)
create a new entry
Definition: index_heap.h:275
Eigen::Matrix< Value, Eigen::Dynamic, 1 > ValueVector
vector of values
Definition: index_heap.h:284
const size_t nbNeighbours
number of neighbours requested
Definition: index_heap.h:80
VT Value
type of a value
Definition: index_heap.h:57
const size_t sizeMinusOne
pre-competed size minus one, to optimise access speed
Definition: index_heap.h:291
Entries data
storage for the tree
Definition: index_heap.h:78
void reset()
reset to the empty heap
Definition: index_heap.h:93
an entry of the heap tree
Definition: index_heap.h:60
void getData(const Eigen::MatrixBase< DI > &indices, const Eigen::MatrixBase< DV > &values) const
get the data from the heap
Definition: index_heap.h:340
void replaceHead(const Index index, const Value value)
put value into heap, replace the largest value if full
Definition: index_heap.h:106
balanced-tree implementation of heap
Definition: index_heap.h:52
IndexHeapSTL(const size_t size)
Constructor.
Definition: index_heap.h:85
void getData(const Eigen::MatrixBase< DI > &indices, const Eigen::MatrixBase< DV > &values) const
get the data from the heap
Definition: index_heap.h:132
IT Index
type of an index
Definition: index_heap.h:264
void reset()
reset to the empty heap
Definition: index_heap.h:303
IT index
index of point
Definition: index_heap.h:271
public interface
Entry(const IT index, const VT value)
create a new entry
Definition: index_heap.h:66
brute-force implementation of heap
Definition: index_heap.h:261
VT Value
type of a value
Definition: index_heap.h:266
VT value
distance for this point
Definition: index_heap.h:63
Eigen::Matrix< Index, Eigen::Dynamic, 1 > IndexVector
vector of indices
Definition: index_heap.h:73
friend bool operator<(const Entry &e0, const Entry &e1)
return true if e0 is of lower value than e1, false otherwise
Definition: index_heap.h:68
IT Index
type of an index
Definition: index_heap.h:55
const VT & headValue() const
get the largest value of the heap
Definition: index_heap.h:101