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  const std::vector<Element*>& getElements(void)
92  {
93  return data_;
94  }
95 
97  Element* add(const _T& d, const double w)
98  {
99  if (w < 0)
100  throw Exception("Weight argument must be a nonnegative value");
101  Element* elem = new Element(d, data_.size());
102  data_.push_back(elem);
103  if (data_.size() == 1)
104  {
105  std::vector<double> r(1, w);
106  tree_.push_back(r);
107  return elem;
108  }
109  tree_.front().push_back(w);
110  for (std::size_t i = 1; i < tree_.size(); ++i)
111  {
112  if (tree_[i-1].size() % 2 == 1)
113  tree_[i].push_back(w);
114  else
115  {
116  while (i < tree_.size())
117  {
118  tree_[i].back() += w;
119  ++i;
120  }
121  return elem;
122  }
123  }
124  //If we've made it here, then we need to add a new head to the tree.
125  std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]);
126  tree_.push_back(head);
127  return elem;
128  }
129 
132  _T& sample(double r) const
133  {
134  if (data_.empty())
135  throw Exception("Cannot sample from an empty PDF");
136  if (r < 0 || r > 1)
137  throw Exception("Sampling value must be between 0 and 1");
138  std::size_t row = tree_.size() - 1;
139  r *= tree_[row].front();
140  std::size_t node = 0;
141  while (row != 0)
142  {
143  --row;
144  node <<= 1;
145  if (r > tree_[row][node])
146  {
147  r -= tree_[row][node];
148  ++node;
149  }
150  }
151  return data_[node]->data_;
152  }
153 
155  void update(Element* elem, const double w)
156  {
157  std::size_t index = elem->index_;
158  if (index >= data_.size())
159  throw Exception("Element to update is not in PDF");
160  const double weightChange = w - tree_.front()[index];
161  tree_.front()[index] = w;
162  index >>= 1;
163  for (std::size_t row = 1; row < tree_.size(); ++row)
164  {
165  tree_[row][index] += weightChange;
166  index >>= 1;
167  }
168  }
169 
171  double getWeight(const Element* elem) const
172  {
173  return tree_.front()[elem->index_];
174  }
175 
177  void remove(Element* elem)
178  {
179  if (data_.size() == 1)
180  {
181  delete data_.front();
182  data_.clear();
183  tree_.clear();
184  return;
185  }
186 
187  const std::size_t index = elem->index_;
188  delete data_[index];
189 
190  double weight;
191  if (index+1 == data_.size())
192  weight = tree_.front().back();
193  else
194  {
195  std::swap(data_[index], data_.back());
196  data_[index]->index_ = index;
197  std::swap(tree_.front()[index], tree_.front().back());
198 
199  /* If index and back() are siblings in the tree, then
200  * we don't need to make an extra pass over the tree.
201  * The amount by which we change the values at the edge
202  * of the tree is different in this case. */
203  if (index+2 == data_.size() && index%2 == 0)
204  weight = tree_.front().back();
205  else
206  {
207  weight = tree_.front()[index];
208  const double weightChange = weight - tree_.front().back();
209  std::size_t parent = index >> 1;
210  for (std::size_t row = 1; row < tree_.size(); ++row)
211  {
212  tree_[row][parent] += weightChange;
213  parent >>= 1;
214  }
215  }
216  }
217 
218  /* Now that the element to remove is at the edge of the tree,
219  * pop it off and update the corresponding weights. */
220  data_.pop_back();
221  tree_.front().pop_back();
222  for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
223  {
224  if (tree_[i-1].size() % 2 == 0)
225  tree_[i].pop_back();
226  else
227  {
228  while (i < tree_.size())
229  {
230  tree_[i].back() -= weight;
231  ++i;
232  }
233  return;
234  }
235  }
236  //If we've made it here, then we need to remove a redundant head from the tree.
237  tree_.pop_back();
238  }
239 
241  void clear(void)
242  {
243  for (typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
244  delete *e;
245  data_.clear();
246  tree_.clear();
247  }
248 
250  std::size_t size(void) const
251  {
252  return data_.size();
253  }
254 
256  const _T& operator[](unsigned int i) const
257  {
258  return data_[i]->data_;
259  }
260 
262  bool empty(void) const
263  {
264  return data_.empty();
265  }
266 
268  void printTree(std::ostream& out = std::cout) const
269  {
270  if (tree_.empty())
271  return;
272  for (std::size_t j = 0; j < tree_[0].size(); ++j)
273  out << "(" << data_[j]->data_ << "," << tree_[0][j] << ") ";
274  out << std::endl;
275  for (std::size_t i = 1; i < tree_.size(); ++i)
276  {
277  for (std::size_t j = 0; j < tree_[i].size(); ++j)
278  out << tree_[i][j] << " ";
279  out << std::endl;
280  }
281  out << std::endl;
282  }
283 
284  private:
285 
286  std::vector<Element*> data_;
287  std::vector<std::vector<double > > tree_;
288  };
289 }
290 
291 #endif
const std::vector< Element * > & getElements(void)
Get the current set of stored elements.
Definition: PDF.h:91
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:171
_T data_
The data contained in this Element.
Definition: PDF.h:58
const _T & operator[](unsigned int i) const
Returns indexed data from the PDF, according to order of insertion.
Definition: PDF.h:256
void clear(void)
Clears the PDF.
Definition: PDF.h:241
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
void printTree(std::ostream &out=std::cout) const
Prints the PDF tree to a given output stream. Used for debugging purposes.
Definition: PDF.h:268
~PDF(void)
Destructor. Clears allocated memory.
Definition: PDF.h:85
A class that will hold data contained in the PDF.
Definition: PDF.h:53
The exception type for ompl.
Definition: Exception.h:47
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
Definition: PDF.h:97
std::size_t size(void) const
Returns the number of elements in the PDF.
Definition: PDF.h:250
PDF(const std::vector< _T > &d, const std::vector< double > &weights)
Constructs a PDF containing a given vector of data with given weights.
Definition: PDF.h:72
bool empty(void) const
Returns whether the PDF contains no data.
Definition: PDF.h:262
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
PDF(void)
Constructs an empty PDF.
Definition: PDF.h:67