Loading...
Searching...
No Matches
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
44namespace ompl
45{
47 template <typename _T>
48 class PDF
49 {
50 public:
52 class Element
53 {
54 friend class PDF;
55
56 public:
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
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
The exception type for ompl.
Definition: Exception.h:47
A class that will hold data contained in the PDF.
Definition: PDF.h:53
_T data_
The data contained in this Element.
Definition: PDF.h:58
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:171
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:263
_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
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:97
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
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:71
const _T & operator[](unsigned int i) const
Returns indexed data from the PDF, according to order of insertion.
Definition: PDF.h:257
const std::vector< Element * > & getElements()
Get the current set of stored elements.
Definition: PDF.h:90
~PDF()
Destructor. Clears allocated memory.
Definition: PDF.h:84
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:178
void printTree(std::ostream &out=std::cout) const
Prints the PDF tree to a given output stream. Used for debugging purposes.
Definition: PDF.h:269
void clear()
Clears the PDF.
Definition: PDF.h:242
std::size_t size() const
Returns the number of elements in the PDF.
Definition: PDF.h:251
PDF()=default
Constructs an empty PDF.
Main namespace. Contains everything in this library.