All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 <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             //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(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             //If we've made it here, then we need to add a new head to the tree.
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             /* If index and back() are siblings in the tree, then
00183              * we don't need to make an extra pass over the tree.
00184              * The amount by which we change the values at the edge
00185              * of the tree is different in this case. */
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             /* Now that the element to remove is at the edge of the tree,
00201              * pop it off and update the corresponding weights. */
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             //If we've made it here, then we need to remove a redundant head from the tree.
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