gtsam  4.1.0
gtsam
BTree.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <stack>
21 #include <sstream>
22 #include <boost/shared_ptr.hpp>
23 #include <boost/function.hpp>
24 
25 namespace gtsam {
26 
31  template<class KEY, class VALUE>
32  class BTree {
33 
34  public:
35 
36  typedef std::pair<KEY, VALUE> value_type;
37 
38  private:
39 
43  struct Node {
44 
45  const size_t height_;
46  const value_type keyValue_;
47  const BTree left, right;
48 
50  Node() {
51  }
52 
57  Node(const value_type& keyValue) :
58  height_(1), keyValue_(keyValue) {
59  }
60 
64  Node(const BTree& l, const value_type& keyValue, const BTree& r) :
65  height_(l.height() >= r.height() ? l.height() + 1 : r.height() + 1),
66  keyValue_(keyValue), left(l), right(r) {
67  }
68 
69  inline const KEY& key() const { return keyValue_.first;}
70  inline const VALUE& value() const { return keyValue_.second;}
71 
72  }; // Node
73 
74  // We store a shared pointer to the root of the functional tree
75  // composed of Node classes. If root_==nullptr, the tree is empty.
76  typedef boost::shared_ptr<const Node> sharedNode;
77  sharedNode root_;
78 
79  inline const value_type& keyValue() const { return root_->keyValue_;}
80  inline const KEY& key() const { return root_->key(); }
81  inline const VALUE& value() const { return root_->value(); }
82  inline const BTree& left() const { return root_->left; }
83  inline const BTree& right() const { return root_->right; }
84 
86  static BTree balance(const BTree& l, const value_type& xd, const BTree& r) {
87  size_t hl = l.height(), hr = r.height();
88  if (hl > hr + 2) {
89  const BTree& ll = l.left(), lr = l.right();
90  if (ll.height() >= lr.height())
91  return BTree(ll, l.keyValue(), BTree(lr, xd, r));
92  else {
93  BTree _left(ll, l.keyValue(), lr.left());
94  BTree _right(lr.right(), xd, r);
95  return BTree(_left, lr.keyValue(), _right);
96  }
97  } else if (hr > hl + 2) {
98  const BTree& rl = r.left(), rr = r.right();
99  if (rr.height() >= rl.height())
100  return BTree(BTree(l, xd, rl), r.keyValue(), rr);
101  else {
102  BTree _left(l, xd, rl.left());
103  BTree _right(rl.right(), r.keyValue(), rr);
104  return BTree(_left, rl.keyValue(), _right);
105  }
106  } else
107  return BTree(l, xd, r);
108  }
109 
110  public:
111 
113  BTree() {
114  }
115 
117  BTree(const BTree& other) :
118  root_(other.root_) {
119  }
120 
122  BTree(const value_type& keyValue) :
123  root_(new Node(keyValue)) {
124  }
125 
127  BTree(const BTree& l, const value_type& keyValue, const BTree& r) :
128  root_(new Node(l, keyValue, r)) {
129  }
130 
132  BTree & operator= (const BTree & other) {
133  root_ = other.root_;
134  return *this;
135  }
136 
138  bool empty() const {
139  return !root_;
140  }
141 
143  BTree add(const value_type& xd) const {
144  if (empty()) return BTree(xd);
145  const KEY& x = xd.first;
146  if (x == key())
147  return BTree(left(), xd, right());
148  else if (x < key())
149  return balance(left().add(xd), keyValue(), right());
150  else
151  return balance(left(), keyValue(), right().add(xd));
152  }
153 
155  BTree add(const KEY& x, const VALUE& d) const {
156  return add(std::make_pair(x, d));
157  }
158 
160  bool mem(const KEY& x) const {
161  if (!root_) return false;
162  if (x == key()) return true;
163  if (x < key())
164  return left().mem(x);
165  else
166  return right().mem(x);
167  }
168 
170  inline bool same(const BTree& other) const {
171  return (other.root_ == root_);
172  }
173 
178  bool operator==(const BTree& other) const {
179  if (other.root_ == root_) return true; // if same, we're done
180  if (empty() && !other.empty()) return false;
181  if (!empty() && other.empty()) return false;
182  // both non-empty, recurse: check this key-value pair and subtrees...
183  return (keyValue() == other.keyValue()) && (left() == other.left())
184  && (right() == other.right());
185  }
186 
187  inline bool operator!=(const BTree& other) const {
188  return !operator==(other);
189  }
190 
192  const value_type& min() const {
193  if (!root_) throw std::invalid_argument("BTree::min: empty tree");
194  if (left().empty()) return keyValue();
195  return left().min();
196  }
197 
199  BTree remove_min() const {
200  if (!root_) throw std::invalid_argument("BTree::remove_min: empty tree");
201  if (left().empty()) return right();
202  return balance(left().remove_min(), keyValue(), right());
203  }
204 
206  static BTree merge(const BTree& t1, const BTree& t2) {
207  if (t1.empty()) return t2;
208  if (t2.empty()) return t1;
209  const value_type& xd = t2.min();
210  return balance(t1, xd, t2.remove_min());
211  }
212 
214  BTree remove(const KEY& x) const {
215  if (!root_) return BTree();
216  if (x == key())
217  return merge(left(), right());
218  else if (x < key())
219  return balance(left().remove(x), keyValue(), right());
220  else
221  return balance(left(), keyValue(), right().remove(x));
222  }
223 
225  size_t height() const {
226  return (root_ != nullptr) ? root_->height_ : 0;
227  }
228 
230  size_t size() const {
231  if (!root_) return 0;
232  return left().size() + 1 + right().size();
233  }
234 
239  const VALUE& find(const KEY& k) const {
240  const Node* node = root_.get();
241  while (node) {
242  const KEY& key = node->key();
243  if (k < key) node = node->left.root_.get();
244  else if (key < k) node = node->right.root_.get();
245  else return node->value();
246  }
247 
248  throw std::invalid_argument("BTree::find: key not found");
249  }
250 
252  void print(const std::string& s = "") const {
253  if (empty()) return;
254  KEY k = key();
255  std::stringstream ss;
256  ss << height();
257  k.print(s + ss.str() + " ");
258  left().print(s + "L ");
259  right().print(s + "R ");
260  }
261 
263  void iter(boost::function<void(const KEY&, const VALUE&)> f) const {
264  if (!root_) return;
265  left().iter(f);
266  f(key(), value());
267  right().iter(f);
268  }
269 
271  template<class TO>
272  BTree<KEY, TO> map(boost::function<TO(const KEY&, const VALUE&)> f) const {
273  if (empty()) return BTree<KEY, TO> ();
274  std::pair<KEY, TO> xd(key(), f(key(), value()));
275  return BTree<KEY, TO> (left().map(f), xd, right().map(f));
276  }
277 
284  template<class ACC>
285  ACC fold(boost::function<ACC(const KEY&, const VALUE&, const ACC&)> f,
286  const ACC& a) const {
287  if (!root_) return a;
288  ACC ar = right().fold(f, a); // fold over right subtree
289  ACC am = f(key(), value(), ar); // apply f with current value
290  return left().fold(f, am); // fold over left subtree
291  }
292 
298 
299  private:
300 
301  typedef const_iterator Self;
302  typedef std::pair<sharedNode, bool> flagged;
303 
305  std::stack<flagged> path_;
306 
307  const sharedNode& current() const {
308  return path_.top().first;
309  }
310 
311  bool done() const {
312  return path_.top().second;
313  }
314 
315  // The idea is we already iterated through the left-subtree and current key-value.
316  // We now try pushing left subtree of right onto the stack. If there is no right
317  // sub-tree, we pop this node of the stack and the parent becomes the iterator.
318  // We avoid going down a right-subtree that was already visited by checking the flag.
319  void increment() {
320  if (path_.empty()) return;
321  sharedNode t = current()->right.root_;
322  if (!t || done()) {
323  // no right subtree, iterator becomes first parent with a non-visited right subtree
324  path_.pop();
325  while (!path_.empty() && done())
326  path_.pop();
327  } else {
328  path_.top().second = true; // flag we visited right
329  // push right root and its left-most path onto the stack
330  while (t) {
331  path_.push(std::make_pair(t, false));
332  t = t->left.root_;
333  }
334  }
335  }
336 
337  public:
338 
339  // traits for playing nice with STL
340  typedef ptrdiff_t difference_type;
341  typedef std::forward_iterator_tag iterator_category;
342  typedef std::pair<KEY, VALUE> value_type;
343  typedef const value_type* pointer;
344  typedef const value_type& reference;
345 
348  }
349 
351  const_iterator(const sharedNode& root) {
352  sharedNode t = root;
353  while (t) {
354  path_.push(std::make_pair(t, false));
355  t = t->left.root_;
356  }
357  }
358 
360  bool operator==(const Self& __x) const {
361  return path_ == __x.path_;
362  }
363 
365  bool operator!=(const Self& __x) const {
366  return path_ != __x.path_;
367  }
368 
370  reference operator*() const {
371  if (path_.empty()) throw std::invalid_argument(
372  "operator*: tried to dereference end");
373  return current()->keyValue_;
374  }
375 
377  pointer operator->() const {
378  if (path_.empty()) throw std::invalid_argument(
379  "operator->: tried to dereference end");
380  return &(current()->keyValue_);
381  }
382 
385  increment();
386  return *this;
387  }
388 
391  Self __tmp = *this;
392  increment();
393  return __tmp;
394  }
395 
396  }; // const_iterator
397 
398  // to make BTree work with range-based for
399  // We do *not* want a non-const iterator
400  typedef const_iterator iterator;
401 
403  const_iterator begin() const {
404  return const_iterator(root_);
405  }
406 
408  const_iterator end() const {
409  return const_iterator();
410  }
411 
412  }; // BTree
413 
414 } // namespace gtsam
415 
gtsam::BTree::operator=
BTree & operator=(const BTree &other)
assignment operator
Definition: BTree.h:132
gtsam::BTree::remove_min
BTree remove_min() const
remove minimum key binding
Definition: BTree.h:199
gtsam::BTree::fold
ACC fold(boost::function< ACC(const KEY &, const VALUE &, const ACC &)> f, const ACC &a) const
t.fold(f,a) computes [(f kN dN ...
Definition: BTree.h:285
gtsam::BTree::iter
void iter(boost::function< void(const KEY &, const VALUE &)> f) const
iterate over tree
Definition: BTree.h:263
gtsam::BTree::operator==
bool operator==(const BTree &other) const
Check whether trees are structurally the same, i.e., contain the same values in same tree-structure.
Definition: BTree.h:178
gtsam::BTree::add
BTree add(const KEY &x, const VALUE &d) const
add a key-value pair
Definition: BTree.h:155
gtsam::BTree::begin
const_iterator begin() const
return iterator
Definition: BTree.h:403
gtsam::BTree::const_iterator::operator*
reference operator*() const
dereference
Definition: BTree.h:370
gtsam::BTree::same
bool same(const BTree &other) const
Check whether trees are exactly the same (occupy same memory)
Definition: BTree.h:170
gtsam::BTree::BTree
BTree(const BTree &other)
copy constructor
Definition: BTree.h:117
gtsam::BTree::BTree
BTree()
default constructor creates an empty tree
Definition: BTree.h:113
gtsam::BTree::mem
bool mem(const KEY &x) const
member predicate
Definition: BTree.h:160
gtsam::BTree
Definition: BTree.h:32
gtsam::BTree::find
const VALUE & find(const KEY &k) const
find a value given a key, throws exception when not found Optimized non-recursive version as [find] i...
Definition: BTree.h:239
gtsam::BTree::const_iterator::operator++
Self operator++(int)
post-increment
Definition: BTree.h:390
gtsam::BTree::const_iterator::operator++
Self & operator++()
pre-increment
Definition: BTree.h:384
gtsam::BTree::const_iterator::operator->
pointer operator->() const
dereference
Definition: BTree.h:377
gtsam::BTree::const_iterator
Const iterator Not trivial: iterator keeps a stack to indicate current path from root_.
Definition: BTree.h:297
gtsam::BTree::height
size_t height() const
Return height of the tree, 0 if empty.
Definition: BTree.h:225
gtsam::BTree::map
BTree< KEY, TO > map(boost::function< TO(const KEY &, const VALUE &)> f) const
map key-values in tree over function f that computes a new value
Definition: BTree.h:272
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::BTree::size
size_t size() const
return size of the tree
Definition: BTree.h:230
gtsam::BTree::BTree
BTree(const BTree &l, const value_type &keyValue, const BTree &r)
create from key-value pair and left, right subtrees
Definition: BTree.h:127
gtsam::BTree::const_iterator::operator==
bool operator==(const Self &__x) const
equality
Definition: BTree.h:360
gtsam::BTree::const_iterator::const_iterator
const_iterator()
initialize end
Definition: BTree.h:347
gtsam::BTree::const_iterator::const_iterator
const_iterator(const sharedNode &root)
initialize from root
Definition: BTree.h:351
gtsam::BTree::const_iterator::operator!=
bool operator!=(const Self &__x) const
inequality
Definition: BTree.h:365
gtsam::BTree::remove
BTree remove(const KEY &x) const
remove a key-value pair
Definition: BTree.h:214
gtsam::BTree::empty
bool empty() const
Check whether tree is empty.
Definition: BTree.h:138
gtsam::BTree::add
BTree add(const value_type &xd) const
add a key-value pair
Definition: BTree.h:143
gtsam::BTree::end
const_iterator end() const
return iterator
Definition: BTree.h:408
gtsam::BTree::merge
static BTree merge(const BTree &t1, const BTree &t2)
merge two trees
Definition: BTree.h:206
gtsam::BTree::print
void print(const std::string &s="") const
print in-order
Definition: BTree.h:252
gtsam::BTree::min
const value_type & min() const
minimum key binding
Definition: BTree.h:192
gtsam::BTree::BTree
BTree(const value_type &keyValue)
create leaf from key-value pair
Definition: BTree.h:122