37 #ifndef OMPL_DATASTRUCTURES_PDF_
38 #define OMPL_DATASTRUCTURES_PDF_
40 #include "ompl/util/Exception.h"
47 template <
typename _T>
61 Element(
const _T &d,
const std::size_t i) :
data_(d), index_(i)
71 PDF(
const std::vector<_T> &d,
const std::vector<double> &weights)
73 if (d.size() != weights.size())
74 throw Exception(
"Data vector and weight vector must be of equal length");
79 for (std::size_t i = 0; i < d.size(); ++i)
80 add(d[i], weights[i]);
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)
105 std::vector<double> r(1, w);
109 tree_.front().push_back(w);
110 for (std::size_t i = 1; i < tree_.size(); ++i)
112 if (tree_[i - 1].
size() % 2 == 1)
113 tree_[i].push_back(w);
116 while (i < tree_.size())
118 tree_[i].back() += w;
125 std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]);
126 tree_.push_back(head);
135 throw Exception(
"Cannot sample from an empty PDF");
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;
145 if (r > tree_[row][node])
147 r -= tree_[row][node];
151 return data_[node]->data_;
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;
163 for (std::size_t row = 1; row < tree_.size(); ++row)
165 tree_[row][index] += weightChange;
173 return tree_.front()[elem->index_];
180 if (data_.size() == 1)
182 delete data_.front();
188 const std::size_t index = elem->index_;
192 if (index + 1 == data_.size())
193 weight = tree_.front().back();
196 std::swap(data_[index], data_.back());
197 data_[index]->index_ = index;
198 std::swap(tree_.front()[index], tree_.front().back());
204 if (index + 2 == data_.size() && index % 2 == 0)
205 weight = tree_.front().back();
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)
213 tree_[row][parent] += weightChange;
222 tree_.front().pop_back();
223 for (std::size_t i = 1; i < tree_.size() && tree_[i - 1].
size() > 1; ++i)
225 if (tree_[i - 1].
size() % 2 == 0)
229 while (i < tree_.size())
231 tree_[i].back() -= weight;
244 for (
auto e = data_.begin(); e != data_.end(); ++e)
259 return data_[i]->data_;
265 return data_.empty();
273 for (std::size_t j = 0; j < tree_[0].size(); ++j)
274 out <<
"(" << data_[j]->data_ <<
"," << tree_[0][j] <<
") ";
276 for (std::size_t i = 1; i < tree_.size(); ++i)
278 for (std::size_t j = 0; j < tree_[i].size(); ++j)
279 out << tree_[i][j] <<
" ";
286 std::vector<Element *> data_;
287 std::vector<std::vector<double>> tree_;
The exception type for ompl.
A class that will hold data contained in the PDF.
_T data_
The data contained in this Element.
A container that supports probabilistic sampling over weighted data.
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
const std::vector< Element * > & getElements()
Get the current set of stored elements.
bool empty() const
Returns whether the PDF contains no data.
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
PDF(const std::vector< _T > &d, const std::vector< double > &weights)
Constructs a PDF containing a given vector of data with given weights.
~PDF()
Destructor. Clears allocated memory.
_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...
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
void printTree(std::ostream &out=std::cout) const
Prints the PDF tree to a given output stream. Used for debugging purposes.
void clear()
Clears the PDF.
std::size_t size() const
Returns the number of elements in the PDF.
const _T & operator[](unsigned int i) const
Returns indexed data from the PDF, according to order of insertion.
PDF()=default
Constructs an empty PDF.
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Main namespace. Contains everything in this library.