00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef OMPL_DATASTRUCTURES_PDF_
00038 #define OMPL_DATASTRUCTURES_PDF_
00039
00040 #include "ompl/util/Exception.h"
00041 #include <iostream>
00042 #include <vector>
00043
00044 namespace ompl
00045 {
00047 template <typename _T>
00048 class PDF
00049 {
00050 public:
00051
00053 class Element
00054 {
00055 friend class PDF;
00056 public:
00058 _T data_;
00059 private:
00060 Element(const _T& d, const std::size_t i) : data_(d), index_(i)
00061 {
00062 }
00063 std::size_t index_;
00064 };
00065
00067 PDF(void)
00068 {
00069 }
00070
00072 PDF(const std::vector<_T>& d, const std::vector<double>& weights)
00073 {
00074 if (d.size() != weights.size())
00075 throw Exception("Data vector and weight vector must be of equal length");
00076
00077 data_.reserve(512u);
00078
00079 tree_.reserve(11u);
00080 for (std::size_t i = 0; i < d.size(); ++i)
00081 add(d[i], weights[i]);
00082 }
00083
00085 ~PDF(void)
00086 {
00087 clear();
00088 }
00089
00091 Element* add(const _T& d, const double w)
00092 {
00093 if (w < 0)
00094 throw Exception("Weight argument must be a nonnegative value");
00095 Element* elem = new Element(d, data_.size());
00096 data_.push_back(elem);
00097 if (data_.size() == 1)
00098 {
00099 std::vector<double> r(1, w);
00100 tree_.push_back(r);
00101 return elem;
00102 }
00103 tree_.front().push_back(w);
00104 for (std::size_t i = 1; i < tree_.size(); ++i)
00105 {
00106 if (tree_[i-1].size() % 2 == 1)
00107 tree_[i].push_back(w);
00108 else
00109 {
00110 while (i < tree_.size())
00111 {
00112 tree_[i].back() += w;
00113 ++i;
00114 }
00115 return elem;
00116 }
00117 }
00118
00119 std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]);
00120 tree_.push_back(head);
00121 return elem;
00122 }
00123
00126 const _T& sample(double r) const
00127 {
00128 if (data_.empty())
00129 throw Exception("Cannot sample from an empty PDF");
00130 if (r < 0 || r > 1)
00131 throw Exception("Sampling value must be between 0 and 1");
00132 std::size_t row = tree_.size() - 1;
00133 r *= tree_[row].front();
00134 std::size_t node = 0;
00135 while (row != 0)
00136 {
00137 --row;
00138 node <<= 1;
00139 if (r > tree_[row][node])
00140 {
00141 r -= tree_[row][node];
00142 ++node;
00143 }
00144 }
00145 return data_[node]->data_;
00146 }
00147
00149 void update(Element* elem, const double w)
00150 {
00151 std::size_t index = elem->index_;
00152 if (index >= data_.size())
00153 throw Exception("Element to update is not in PDF");
00154 const double weightChange = w - tree_.front()[index];
00155 tree_.front()[index] = w;
00156 index >>= 1;
00157 for (std::size_t row = 1; row < tree_.size(); ++row)
00158 {
00159 tree_[row][index] += weightChange;
00160 index >>= 1;
00161 }
00162 }
00163
00165 double getWeight(const Element* elem) const
00166 {
00167 return tree_.front()[elem->index_];
00168 }
00169
00171 void remove(Element* elem)
00172 {
00173 if (data_.size() == 1)
00174 {
00175 delete data_.front();
00176 data_.clear();
00177 tree_.clear();
00178 return;
00179 }
00180
00181 const std::size_t index = elem->index_;
00182 delete data_[index];
00183
00184 double weight;
00185 if (index+1 == data_.size())
00186 weight = tree_.front().back();
00187 else
00188 {
00189 std::swap(data_[index], data_.back());
00190 data_[index]->index_ = index;
00191 std::swap(tree_.front()[index], tree_.front().back());
00192
00193
00194
00195
00196
00197 if (index+2 == data_.size() && index%2 == 0)
00198 weight = tree_.front().back();
00199 else
00200 {
00201 weight = tree_.front()[index];
00202 const double weightChange = weight - tree_.front().back();
00203 std::size_t parent = index >> 1;
00204 for (std::size_t row = 1; row < tree_.size(); ++row)
00205 {
00206 tree_[row][parent] += weightChange;
00207 parent >>= 1;
00208 }
00209 }
00210 }
00211
00212
00213
00214 data_.pop_back();
00215 tree_.front().pop_back();
00216 for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
00217 {
00218 if (tree_[i-1].size() % 2 == 0)
00219 tree_[i].pop_back();
00220 else
00221 {
00222 while (i < tree_.size())
00223 {
00224 tree_[i].back() -= weight;
00225 ++i;
00226 }
00227 return;
00228 }
00229 }
00230
00231 tree_.pop_back();
00232 }
00233
00235 void clear(void)
00236 {
00237 for (typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
00238 delete *e;
00239 data_.clear();
00240 tree_.clear();
00241 }
00242
00244 std::size_t size(void) const
00245 {
00246 return data_.size();
00247 }
00248
00250 bool empty(void) const
00251 {
00252 return data_.empty();
00253 }
00254
00256 void printTree(std::ostream& out = std::cout) const
00257 {
00258 if (tree_.empty())
00259 return;
00260 for (std::size_t j = 0; j < tree_[0].size(); ++j)
00261 out << "(" << data_[j]->data_ << "," << tree_[0][j] << ") ";
00262 out << std::endl;
00263 for (std::size_t i = 1; i < tree_.size(); ++i)
00264 {
00265 for (std::size_t j = 0; j < tree_[i].size(); ++j)
00266 out << tree_[i][j] << " ";
00267 out << std::endl;
00268 }
00269 out << std::endl;
00270 }
00271
00272 private:
00273
00274 std::vector<Element*> data_;
00275 std::vector<std::vector<double > > tree_;
00276 };
00277 }
00278
00279 #endif