All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PDF.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #ifndef OMPL_DATASTRUCTURES_PDF_
38 #define OMPL_DATASTRUCTURES_PDF_
39 
40 #include "ompl/util/Exception.h"
41 #include <iostream>
42 #include <vector>
43 
44 namespace ompl
45 {
47  template <typename _T>
48  class PDF
49  {
50  public:
51 
53  class Element
54  {
55  friend class PDF;
56  public:
58  _T data_;
59  private:
60  Element(const _T& d, const std::size_t i) : data_(d), index_(i)
61  {
62  }
63  std::size_t index_;
64  };
65 
67  PDF(void)
68  {
69  }
70 
72  PDF(const std::vector<_T>& d, const std::vector<double>& weights)
73  {
74  if (d.size() != weights.size())
75  throw Exception("Data vector and weight vector must be of equal length");
76  //by default, reserve space for 512 elements
77  data_.reserve(512u);
78  //n elements require at most log2(n)+2 rows of the tree
79  tree_.reserve(11u);
80  for (std::size_t i = 0; i < d.size(); ++i)
81  add(d[i], weights[i]);
82  }
83 
85  ~PDF(void)
86  {
87  clear();
88  }
89 
91  Element* add(const _T& d, const double w)
92  {
93  if (w < 0)
94  throw Exception("Weight argument must be a nonnegative value");
95  Element* elem = new Element(d, data_.size());
96  data_.push_back(elem);
97  if (data_.size() == 1)
98  {
99  std::vector<double> r(1, w);
100  tree_.push_back(r);
101  return elem;
102  }
103  tree_.front().push_back(w);
104  for (std::size_t i = 1; i < tree_.size(); ++i)
105  {
106  if (tree_[i-1].size() % 2 == 1)
107  tree_[i].push_back(w);
108  else
109  {
110  while (i < tree_.size())
111  {
112  tree_[i].back() += w;
113  ++i;
114  }
115  return elem;
116  }
117  }
118  //If we've made it here, then we need to add a new head to the tree.
119  std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]);
120  tree_.push_back(head);
121  return elem;
122  }
123 
126  const _T& sample(double r) const
127  {
128  if (data_.empty())
129  throw Exception("Cannot sample from an empty PDF");
130  if (r < 0 || r > 1)
131  throw Exception("Sampling value must be between 0 and 1");
132  std::size_t row = tree_.size() - 1;
133  r *= tree_[row].front();
134  std::size_t node = 0;
135  while (row != 0)
136  {
137  --row;
138  node <<= 1;
139  if (r > tree_[row][node])
140  {
141  r -= tree_[row][node];
142  ++node;
143  }
144  }
145  return data_[node]->data_;
146  }
147 
149  void update(Element* elem, const double w)
150  {
151  std::size_t index = elem->index_;
152  if (index >= data_.size())
153  throw Exception("Element to update is not in PDF");
154  const double weightChange = w - tree_.front()[index];
155  tree_.front()[index] = w;
156  index >>= 1;
157  for (std::size_t row = 1; row < tree_.size(); ++row)
158  {
159  tree_[row][index] += weightChange;
160  index >>= 1;
161  }
162  }
163 
165  double getWeight(const Element* elem) const
166  {
167  return tree_.front()[elem->index_];
168  }
169 
171  void remove(Element* elem)
172  {
173  if (data_.size() == 1)
174  {
175  delete data_.front();
176  data_.clear();
177  tree_.clear();
178  return;
179  }
180 
181  const std::size_t index = elem->index_;
182  delete data_[index];
183 
184  double weight;
185  if (index+1 == data_.size())
186  weight = tree_.front().back();
187  else
188  {
189  std::swap(data_[index], data_.back());
190  data_[index]->index_ = index;
191  std::swap(tree_.front()[index], tree_.front().back());
192 
193  /* If index and back() are siblings in the tree, then
194  * we don't need to make an extra pass over the tree.
195  * The amount by which we change the values at the edge
196  * of the tree is different in this case. */
197  if (index+2 == data_.size() && index%2 == 0)
198  weight = tree_.front().back();
199  else
200  {
201  weight = tree_.front()[index];
202  const double weightChange = weight - tree_.front().back();
203  std::size_t parent = index >> 1;
204  for (std::size_t row = 1; row < tree_.size(); ++row)
205  {
206  tree_[row][parent] += weightChange;
207  parent >>= 1;
208  }
209  }
210  }
211 
212  /* Now that the element to remove is at the edge of the tree,
213  * pop it off and update the corresponding weights. */
214  data_.pop_back();
215  tree_.front().pop_back();
216  for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
217  {
218  if (tree_[i-1].size() % 2 == 0)
219  tree_[i].pop_back();
220  else
221  {
222  while (i < tree_.size())
223  {
224  tree_[i].back() -= weight;
225  ++i;
226  }
227  return;
228  }
229  }
230  //If we've made it here, then we need to remove a redundant head from the tree.
231  tree_.pop_back();
232  }
233 
235  void clear(void)
236  {
237  for (typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
238  delete *e;
239  data_.clear();
240  tree_.clear();
241  }
242 
244  std::size_t size(void) const
245  {
246  return data_.size();
247  }
248 
250  bool empty(void) const
251  {
252  return data_.empty();
253  }
254 
256  void printTree(std::ostream& out = std::cout) const
257  {
258  if (tree_.empty())
259  return;
260  for (std::size_t j = 0; j < tree_[0].size(); ++j)
261  out << "(" << data_[j]->data_ << "," << tree_[0][j] << ") ";
262  out << std::endl;
263  for (std::size_t i = 1; i < tree_.size(); ++i)
264  {
265  for (std::size_t j = 0; j < tree_[i].size(); ++j)
266  out << tree_[i][j] << " ";
267  out << std::endl;
268  }
269  out << std::endl;
270  }
271 
272  private:
273 
274  std::vector<Element*> data_;
275  std::vector<std::vector<double > > tree_;
276  };
277 }
278 
279 #endif