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:
52  class Element
53  {
54  friend class PDF;
55 
56  public:
58  _T data_;
59 
60  private:
61  Element(const _T &d, const std::size_t i) : data_(d), index_(i)
62  {
63  }
64  std::size_t index_;
65  };
66 
68  PDF() = default;
69 
71  PDF(const std::vector<_T> &d, const std::vector<double> &weights)
72  {
73  if (d.size() != weights.size())
74  throw Exception("Data vector and weight vector must be of equal length");
75  // by default, reserve space for 512 elements
76  data_.reserve(512u);
77  // n elements require at most log2(n)+2 rows of the tree
78  tree_.reserve(11u);
79  for (std::size_t i = 0; i < d.size(); ++i)
80  add(d[i], weights[i]);
81  }
82 
84  ~PDF()
85  {
86  clear();
87  }
88 
90  const std::vector<Element *> &getElements()
91  {
92  return data_;
93  }
94 
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  auto *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 
178  void remove(Element *elem)
179  {
180  if (data_.size() == 1)
181  {
182  delete data_.front();
183  data_.clear();
184  tree_.clear();
185  return;
186  }
187 
188  const std::size_t index = elem->index_;
189  delete data_[index];
190 
191  double weight;
192  if (index + 1 == data_.size())
193  weight = tree_.front().back();
194  else
195  {
196  std::swap(data_[index], data_.back());
197  data_[index]->index_ = index;
198  std::swap(tree_.front()[index], tree_.front().back());
199 
200  /* If index and back() are siblings in the tree, then
201  * we don't need to make an extra pass over the tree.
202  * The amount by which we change the values at the edge
203  * of the tree is different in this case. */
204  if (index + 2 == data_.size() && index % 2 == 0)
205  weight = tree_.front().back();
206  else
207  {
208  weight = tree_.front()[index];
209  const double weightChange = weight - tree_.front().back();
210  std::size_t parent = index >> 1;
211  for (std::size_t row = 1; row < tree_.size(); ++row)
212  {
213  tree_[row][parent] += weightChange;
214  parent >>= 1;
215  }
216  }
217  }
218 
219  /* Now that the element to remove is at the edge of the tree,
220  * pop it off and update the corresponding weights. */
221  data_.pop_back();
222  tree_.front().pop_back();
223  for (std::size_t i = 1; i < tree_.size() && tree_[i - 1].size() > 1; ++i)
224  {
225  if (tree_[i - 1].size() % 2 == 0)
226  tree_[i].pop_back();
227  else
228  {
229  while (i < tree_.size())
230  {
231  tree_[i].back() -= weight;
232  ++i;
233  }
234  return;
235  }
236  }
237  // If we've made it here, then we need to remove a redundant head from the tree.
238  tree_.pop_back();
239  }
240 
242  void clear()
243  {
244  for (auto e = data_.begin(); e != data_.end(); ++e)
245  delete *e;
246  data_.clear();
247  tree_.clear();
248  }
249 
251  std::size_t size() const
252  {
253  return data_.size();
254  }
255 
257  const _T &operator[](unsigned int i) const
258  {
259  return data_[i]->data_;
260  }
261 
263  bool empty() const
264  {
265  return data_.empty();
266  }
267 
269  void printTree(std::ostream &out = std::cout) const
270  {
271  if (tree_.empty())
272  return;
273  for (std::size_t j = 0; j < tree_[0].size(); ++j)
274  out << "(" << data_[j]->data_ << "," << tree_[0][j] << ") ";
275  out << std::endl;
276  for (std::size_t i = 1; i < tree_.size(); ++i)
277  {
278  for (std::size_t j = 0; j < tree_[i].size(); ++j)
279  out << tree_[i][j] << " ";
280  out << std::endl;
281  }
282  out << std::endl;
283  }
284 
285  private:
286  std::vector<Element *> data_;
287  std::vector<std::vector<double>> tree_;
288  };
289 }
290 
291 #endif
_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:196
void printTree(std::ostream &out=std::cout) const
Prints the PDF tree to a given output stream. Used for debugging purposes.
Definition: PDF.h:333
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:161
std::size_t size() const
Returns the number of elements in the PDF.
Definition: PDF.h:315
PDF()=default
Constructs an empty PDF.
const std::vector< Element * > & getElements()
Get the current set of stored elements.
Definition: PDF.h:154
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:327
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
Definition: PDF.h:242
_T data_
The data contained in this Element.
Definition: PDF.h:154
void clear()
Clears the PDF.
Definition: PDF.h:306
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:235
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:219
const _T & operator[](unsigned int i) const
Returns indexed data from the PDF, according to order of insertion.
Definition: PDF.h:321
The exception type for ompl.
Definition: Exception.h:78
~PDF()
Destructor. Clears allocated memory.
Definition: PDF.h:148
Main namespace. Contains everything in this library.