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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines