37 #ifndef OMPL_DATASTRUCTURES_PDF_
38 #define OMPL_DATASTRUCTURES_PDF_
40 #include "ompl/util/Exception.h"
47 template <
typename _T>
60 Element(
const _T& d,
const std::size_t i) :
data_(d), index_(i)
72 PDF(
const std::vector<_T>& d,
const std::vector<double>& weights)
74 if (d.size() != weights.size())
75 throw Exception(
"Data vector and weight vector must be of equal length");
80 for (std::size_t i = 0; i < d.size(); ++i)
81 add(d[i], weights[i]);
91 Element*
add(
const _T& d,
const double w)
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)
99 std::vector<double> r(1, w);
103 tree_.front().push_back(w);
104 for (std::size_t i = 1; i < tree_.size(); ++i)
106 if (tree_[i-1].
size() % 2 == 1)
107 tree_[i].push_back(w);
110 while (i < tree_.size())
112 tree_[i].back() += w;
119 std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]);
120 tree_.push_back(head);
129 throw Exception(
"Cannot sample from an empty PDF");
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;
139 if (r > tree_[row][node])
141 r -= tree_[row][node];
145 return data_[node]->data_;
149 void update(Element* elem,
const double w)
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;
157 for (std::size_t row = 1; row < tree_.size(); ++row)
159 tree_[row][index] += weightChange;
167 return tree_.front()[elem->index_];
171 void remove(Element* elem)
173 if (data_.size() == 1)
175 delete data_.front();
181 const std::size_t index = elem->index_;
185 if (index+1 == data_.size())
186 weight = tree_.front().back();
189 std::swap(data_[index], data_.back());
190 data_[index]->index_ = index;
191 std::swap(tree_.front()[index], tree_.front().back());
197 if (index+2 == data_.size() && index%2 == 0)
198 weight = tree_.front().back();
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)
206 tree_[row][parent] += weightChange;
215 tree_.front().pop_back();
216 for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
218 if (tree_[i-1].
size() % 2 == 0)
222 while (i < tree_.size())
224 tree_[i].back() -= weight;
237 for (
typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
252 return data_.empty();
260 for (std::size_t j = 0; j < tree_[0].size(); ++j)
261 out <<
"(" << data_[j]->data_ <<
"," << tree_[0][j] <<
") ";
263 for (std::size_t i = 1; i < tree_.size(); ++i)
265 for (std::size_t j = 0; j < tree_[i].size(); ++j)
266 out << tree_[i][j] <<
" ";
274 std::vector<Element*> data_;
275 std::vector<std::vector<double > > tree_;