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 <ostream>
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 void remove(Element& elem)
00166 {
00167 if (data_.size() == 1)
00168 {
00169 delete data_.front();
00170 data_.clear();
00171 tree_.clear();
00172 return;
00173 }
00174
00175 const std::size_t index = elem.index_;
00176 delete data_[index];
00177 std::swap(data_[index], data_.back());
00178 data_[index]->index_ = index;
00179 std::swap(tree_.front()[index], tree_.front().back());
00180
00181 double weight;
00182
00183
00184
00185
00186 if (index+2 == data_.size() && index%2 == 0)
00187 weight = tree_.front().back();
00188 else
00189 {
00190 weight = tree_.front()[index];
00191 const double weightChange = weight - tree_.front().back();
00192 std::size_t parent = index >> 1;
00193 for (std::size_t row = 1; row < tree_.size(); ++row)
00194 {
00195 tree_[row][parent] += weightChange;
00196 parent >>= 1;
00197 }
00198 }
00199
00200
00201
00202 data_.pop_back();
00203 tree_.front().pop_back();
00204 for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i)
00205 {
00206 if (tree_[i-1].size() % 2 == 0)
00207 tree_[i].pop_back();
00208 else
00209 {
00210 while (i < tree_.size())
00211 {
00212 tree_[i].back() -= weight;
00213 ++i;
00214 }
00215 return;
00216 }
00217 }
00218
00219 tree_.pop_back();
00220 }
00221
00223 void clear(void)
00224 {
00225 for (typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e)
00226 delete *e;
00227 data_.clear();
00228 tree_.clear();
00229 }
00230
00232 std::size_t size(void) const
00233 {
00234 return data_.size();
00235 }
00236
00238 bool empty(void) const
00239 {
00240 return data_.empty();
00241 }
00242
00244 void printTree(std::ostream& out = std::cout) const
00245 {
00246 if (tree_.empty())
00247 return;
00248 for (std::size_t j = 0; j < tree_[0].size(); ++j)
00249 out << "(" << data_[j]->data << "," << tree_[0][j] << ") ";
00250 out << std::endl;
00251 for (std::size_t i = 1; i < tree_.size(); ++i)
00252 {
00253 for (std::size_t j = 0; j < tree_[i].size(); ++j)
00254 out << tree_[i][j] << " ";
00255 out << std::endl;
00256 }
00257 out << std::endl;
00258 }
00259
00260 private:
00261
00262 std::vector<Element*> data_;
00263 std::vector<std::vector<double > > tree_;
00264 };
00265 }
00266
00267 #endif