ompl/datastructures/PDF.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Matt Maly */ 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() 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 //by default, reserve space for 512 elements 00077 data_.reserve(512u); 00078 //n elements require at most log2(n)+2 rows of the tree 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() 00086 { 00087 clear(); 00088 } 00089 00091 const std::vector<Element*>& getElements() 00092 { 00093 return data_; 00094 } 00095 00097 Element* add(const _T& d, const double w) 00098 { 00099 if (w < 0) 00100 throw Exception("Weight argument must be a nonnegative value"); 00101 Element *elem = new Element(d, data_.size()); 00102 data_.push_back(elem); 00103 if (data_.size() == 1) 00104 { 00105 std::vector<double> r(1, w); 00106 tree_.push_back(r); 00107 return elem; 00108 } 00109 tree_.front().push_back(w); 00110 for (std::size_t i = 1; i < tree_.size(); ++i) 00111 { 00112 if (tree_[i-1].size() % 2 == 1) 00113 tree_[i].push_back(w); 00114 else 00115 { 00116 while (i < tree_.size()) 00117 { 00118 tree_[i].back() += w; 00119 ++i; 00120 } 00121 return elem; 00122 } 00123 } 00124 //If we've made it here, then we need to add a new head to the tree. 00125 std::vector<double> head(1, tree_.back()[0] + tree_.back()[1]); 00126 tree_.push_back(head); 00127 return elem; 00128 } 00129 00132 _T& sample(double r) const 00133 { 00134 if (data_.empty()) 00135 throw Exception("Cannot sample from an empty PDF"); 00136 if (r < 0 || r > 1) 00137 throw Exception("Sampling value must be between 0 and 1"); 00138 std::size_t row = tree_.size() - 1; 00139 r *= tree_[row].front(); 00140 std::size_t node = 0; 00141 while (row != 0) 00142 { 00143 --row; 00144 node <<= 1; 00145 if (r > tree_[row][node]) 00146 { 00147 r -= tree_[row][node]; 00148 ++node; 00149 } 00150 } 00151 return data_[node]->data_; 00152 } 00153 00155 void update(Element *elem, const double w) 00156 { 00157 std::size_t index = elem->index_; 00158 if (index >= data_.size()) 00159 throw Exception("Element to update is not in PDF"); 00160 const double weightChange = w - tree_.front()[index]; 00161 tree_.front()[index] = w; 00162 index >>= 1; 00163 for (std::size_t row = 1; row < tree_.size(); ++row) 00164 { 00165 tree_[row][index] += weightChange; 00166 index >>= 1; 00167 } 00168 } 00169 00171 double getWeight(const Element *elem) const 00172 { 00173 return tree_.front()[elem->index_]; 00174 } 00175 00177 void remove(Element *elem) 00178 { 00179 if (data_.size() == 1) 00180 { 00181 delete data_.front(); 00182 data_.clear(); 00183 tree_.clear(); 00184 return; 00185 } 00186 00187 const std::size_t index = elem->index_; 00188 delete data_[index]; 00189 00190 double weight; 00191 if (index+1 == data_.size()) 00192 weight = tree_.front().back(); 00193 else 00194 { 00195 std::swap(data_[index], data_.back()); 00196 data_[index]->index_ = index; 00197 std::swap(tree_.front()[index], tree_.front().back()); 00198 00199 /* If index and back() are siblings in the tree, then 00200 * we don't need to make an extra pass over the tree. 00201 * The amount by which we change the values at the edge 00202 * of the tree is different in this case. */ 00203 if (index+2 == data_.size() && index%2 == 0) 00204 weight = tree_.front().back(); 00205 else 00206 { 00207 weight = tree_.front()[index]; 00208 const double weightChange = weight - tree_.front().back(); 00209 std::size_t parent = index >> 1; 00210 for (std::size_t row = 1; row < tree_.size(); ++row) 00211 { 00212 tree_[row][parent] += weightChange; 00213 parent >>= 1; 00214 } 00215 } 00216 } 00217 00218 /* Now that the element to remove is at the edge of the tree, 00219 * pop it off and update the corresponding weights. */ 00220 data_.pop_back(); 00221 tree_.front().pop_back(); 00222 for (std::size_t i = 1; i < tree_.size() && tree_[i-1].size() > 1; ++i) 00223 { 00224 if (tree_[i-1].size() % 2 == 0) 00225 tree_[i].pop_back(); 00226 else 00227 { 00228 while (i < tree_.size()) 00229 { 00230 tree_[i].back() -= weight; 00231 ++i; 00232 } 00233 return; 00234 } 00235 } 00236 //If we've made it here, then we need to remove a redundant head from the tree. 00237 tree_.pop_back(); 00238 } 00239 00241 void clear() 00242 { 00243 for (typename std::vector<Element*>::iterator e = data_.begin(); e != data_.end(); ++e) 00244 delete *e; 00245 data_.clear(); 00246 tree_.clear(); 00247 } 00248 00250 std::size_t size() const 00251 { 00252 return data_.size(); 00253 } 00254 00256 const _T& operator[](unsigned int i) const 00257 { 00258 return data_[i]->data_; 00259 } 00260 00262 bool empty() const 00263 { 00264 return data_.empty(); 00265 } 00266 00268 void printTree(std::ostream& out = std::cout) const 00269 { 00270 if (tree_.empty()) 00271 return; 00272 for (std::size_t j = 0; j < tree_[0].size(); ++j) 00273 out << "(" << data_[j]->data_ << "," << tree_[0][j] << ") "; 00274 out << std::endl; 00275 for (std::size_t i = 1; i < tree_.size(); ++i) 00276 { 00277 for (std::size_t j = 0; j < tree_[i].size(); ++j) 00278 out << tree_[i][j] << " "; 00279 out << std::endl; 00280 } 00281 out << std::endl; 00282 } 00283 00284 private: 00285 00286 std::vector<Element*> data_; 00287 std::vector<std::vector<double > > tree_; 00288 }; 00289 } 00290 00291 #endif