index_heap.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
00004 You can contact the author at <stephane at magnenat dot net>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the <organization> nor the
00016       names of its contributors may be used to endorse or promote products
00017       derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef __INDEX_HEAP_H
00033 #define __INDEX_HEAP_H
00034 
00035 #include "nabo.h"
00036 #include <vector>
00037 #include <algorithm>
00038 #include <limits>
00039 #include <iostream>
00040 
00046 namespace Nabo
00047 {
00049 
00051         template<typename IT, typename VT>
00052         struct IndexHeapSTL
00053         {
00055                 typedef IT Index;
00057                 typedef VT Value;
00058                 
00060                 struct Entry
00061                 {
00062                         IT index; 
00063                         VT value; 
00064                         
00066                         Entry(const IT index, const VT value): index(index), value(value) {}
00068                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00069                 };
00071                 typedef std::vector<Entry> Entries;
00073                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00075                 typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
00076                 
00078                 Entries data;
00080                 const size_t nbNeighbours;
00081 
00082                 
00084 
00085                 IndexHeapSTL(const size_t size):
00086                         data(1, Entry(0, std::numeric_limits<VT>::infinity())),
00087                         nbNeighbours(size)
00088                 {
00089                         data.reserve(size);
00090                 }
00091                 
00093                 inline void reset()
00094                 {
00095                         data.clear();
00096                         data.push_back(Entry(0, std::numeric_limits<VT>::infinity()));
00097                 }
00098                 
00100 
00101                 inline const VT& headValue() const { return data.front().value; }
00102                 
00104 
00106                 inline void replaceHead(const Index index, const Value value)
00107                 {
00108 
00109                         if (data.size() == nbNeighbours)
00110                         {       // we have enough neighbours to discard largest
00111                                 pop_heap(data.begin(), data.end());
00112                                 data.back() = Entry(index, value);
00113                         }
00114                         else
00115                         {       // missing neighbours
00116                                 data.push_back(Entry(index, value));
00117                         }
00118                         // ensure heap
00119                         push_heap(data.begin(), data.end());
00120                 }
00121                 
00123                 inline void sort()
00124                 {
00125                         sort_heap (data.begin(), data.end());
00126                 }
00127                 
00129 
00131                 template<typename DI, typename DV>
00132                 inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
00133                 {
00134                         // note: we must implement this hack because of problem with reference to temporary
00135                         // C++0x will solve this with rvalue
00136                         // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
00137                         // for more informations
00138                         size_t i = 0;
00139                         for (; i < data.size(); ++i)
00140                         {
00141                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
00142                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
00143                         }
00144                         for (; i < nbNeighbours; ++i)
00145                         {
00146                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = 0;
00147                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = std::numeric_limits<VT>::infinity();
00148                         }
00149                 }
00150                 
00151 #if 0
00152 
00153 
00154                 inline IndexVector getIndexes() const
00155                 {
00156                         IndexVector indexes(data.capacity());
00157                         size_t i = 0;
00158                         for (; i < data.size(); ++i)
00159                                 indexes.coeffRef(i) = data[i].index;
00160                         for (; i < data.capacity(); ++i)
00161                                 indexes.coeffRef(i) = 0;
00162                         return indexes;
00163                 }
00164 #endif
00165         };
00166         
00167 #if 0
00168 
00169 
00171         template<typename IT, typename VT>
00172         struct IndexHeapBruteForceVector
00173         {
00175                 typedef IT Index;
00177                 typedef VT Value;
00178                 
00180                 struct Entry
00181                 {
00182                         IT index; 
00183                         VT value;  
00184                         
00186                         Entry(const IT index, const VT value): index(index), value(value) {} 
00188                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00189                 };
00191                 typedef std::vector<Entry> Entries;
00193                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00194                 
00196                 Entries data;
00198                 const VT& headValueRef;
00200                 const size_t sizeMinusOne;
00201                 
00203 
00204                 IndexHeapBruteForceVector(const size_t size):
00205                         data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00206                         headValueRef((data.end() - 1)->value),
00207                         sizeMinusOne(data.size() - 1)
00208                 {
00209                 }
00210                 
00212                 inline void reset()
00213                 {
00214                         for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
00215                         {
00216                                 it->value = std::numeric_limits<VT>::infinity();
00217                                 it->index = 0;
00218                         }
00219                 }
00220                 
00222 
00223                 inline const VT& headValue() const { return data[0].value; }
00224                 
00226 
00228                 inline void replaceHead(const Index index, const Value value)
00229                 {
00230                         register size_t i = 0;
00231                         for (; i < sizeMinusOne; ++i)
00232                         {
00233                                 if (data[i + 1].value > value)
00234                                         data[i] = data[i + 1];
00235                                 else
00236                                         break;
00237                         }
00238                         data[i].value = value;
00239                         data[i].index = index;
00240                 }
00241                 
00243                 inline void sort()
00244                 {
00245                         // no need to sort as data are already sorted
00246                 }
00247                 
00249 
00250                 inline IndexVector getIndexes() const
00251                 {
00252                         IndexVector indexes(data.size());
00253                         for (size_t i = 0; i < data.size(); ++i)
00254                                 indexes.coeffRef(i) = data[sizeMinusOne-i].index;
00255                         return indexes;
00256                 }
00257         };
00258 #endif
00259         
00261 
00263         template<typename IT, typename VT>
00264         struct IndexHeapBruteForceVector
00265         {
00267                 typedef IT Index;
00269                 typedef VT Value;
00270                 
00272                 struct Entry
00273                 {
00274                         IT index; 
00275                         VT value;  
00276                         
00278                         Entry(const IT index, const VT value): index(index), value(value) {} 
00280                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00281                 };
00283                 typedef std::vector<Entry> Entries;
00285                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00287                 typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
00288                 
00290                 Entries data;
00292                 const VT& headValueRef;
00294                 const size_t sizeMinusOne;
00295                 
00297 
00298                 IndexHeapBruteForceVector(const size_t size):
00299                 data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00300                 headValueRef((data.end() - 1)->value),
00301                 sizeMinusOne(data.size() - 1)
00302                 {
00303                 }
00304                 
00306                 inline void reset()
00307                 {
00308                         for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
00309                         {
00310                                 it->value = std::numeric_limits<VT>::infinity();
00311                                 it->index = 0;
00312                         }
00313                 }
00314                 
00316 
00317                 inline const VT& headValue() const { return headValueRef; }
00318                 
00320 
00322                 inline void replaceHead(const Index index, const Value value)
00323                 {
00324                         register size_t i;
00325                         for (i = sizeMinusOne; i > 0; --i)
00326                         {
00327                                 if (data[i-1].value > value)
00328                                         data[i] = data[i-1];
00329                                 else
00330                                         break;
00331                         }
00332                         data[i].value = value;
00333                         data[i].index = index;
00334                 }
00335                 
00337                 inline void sort()
00338                 {
00339                         // no need to sort as data are already sorted
00340                 }
00341                 
00343 
00345                 template<typename DI, typename DV>
00346                 inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
00347                 {
00348                         // note: we must implement this hack because of problem with reference to temporary
00349                         // C++0x will solve this with rvalue
00350                         // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
00351                         // for more informations
00352                         for (size_t i = 0; i < data.size(); ++i)
00353                         {
00354                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
00355                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
00356                         }
00357                 }
00358 #if 0
00359 
00360 
00361                 inline IndexVector getIndexes() const
00362                 {
00363                         IndexVector indexes(data.size());
00364                         for (size_t i = 0; i < data.size(); ++i)
00365                                 indexes.coeffRef(i) = data[i].index;
00366                         return indexes;
00367                 }
00368 #endif
00369         };
00370 }
00371 
00372 #endif // __INDEX_HEAP_H


libnabo
Author(s): Stéphane Magnenat
autogenerated on Thu Sep 10 2015 10:54:55