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]);
97 Element*
add(
const _T& d,
const double w)
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)
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_;
155 void update(Element* elem,
const double w)
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_];
177 void remove(Element* elem)
179 if (data_.size() == 1)
181 delete data_.front();
187 const std::size_t index = elem->index_;
191 if (index+1 == data_.size())
192 weight = tree_.front().back();
195 std::swap(data_[index], data_.back());
196 data_[index]->index_ = index;
197 std::swap(tree_.front()[index], tree_.front().back());
203 if (index+2 == data_.size() && index%2 == 0)
204 weight = tree_.front().back();
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)
212 tree_[row][parent] += weightChange;
221 tree_.front().pop_back();
222 for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
224 if (tree_[i-1].
size() % 2 == 0)
228 while (i < tree_.size())
230 tree_[i].back() -= weight;
243 for (
typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
258 return data_.empty();
266 for (std::size_t j = 0; j < tree_[0].size(); ++j)
267 out <<
"(" << data_[j]->data_ <<
"," << tree_[0][j] <<
") ";
269 for (std::size_t i = 1; i < tree_.size(); ++i)
271 for (std::size_t j = 0; j < tree_[i].size(); ++j)
272 out << tree_[i][j] <<
" ";
280 std::vector<Element*> data_;
281 std::vector<std::vector<double > > tree_;