libnabo  1.0.4
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                                 it->value = std::numeric_limits<VT>::infinity();
00216                 }
00217                 
00219 
00220                 inline const VT& headValue() const { return data[0].value; }
00221                 
00223 
00225                 inline void replaceHead(const Index index, const Value value)
00226                 {
00227                         register size_t i = 0;
00228                         for (; i < sizeMinusOne; ++i)
00229                         {
00230                                 if (data[i + 1].value > value)
00231                                         data[i] = data[i + 1];
00232                                 else
00233                                         break;
00234                         }
00235                         data[i].value = value;
00236                         data[i].index = index;
00237                 }
00238                 
00240                 inline void sort()
00241                 {
00242                         // no need to sort as data are already sorted
00243                 }
00244                 
00246 
00247                 inline IndexVector getIndexes() const
00248                 {
00249                         IndexVector indexes(data.size());
00250                         for (size_t i = 0; i < data.size(); ++i)
00251                                 indexes.coeffRef(i) = data[sizeMinusOne-i].index;
00252                         return indexes;
00253                 }
00254         };
00255 #endif
00256         
00258 
00260         template<typename IT, typename VT>
00261         struct IndexHeapBruteForceVector
00262         {
00264                 typedef IT Index;
00266                 typedef VT Value;
00267                 
00269                 struct Entry
00270                 {
00271                         IT index; 
00272                         VT value;  
00273                         
00275                         Entry(const IT index, const VT value): index(index), value(value) {} 
00277                         friend bool operator<(const Entry& e0, const Entry& e1) { return e0.value < e1.value; }
00278                 };
00280                 typedef std::vector<Entry> Entries;
00282                 typedef typename Eigen::Matrix<Index, Eigen::Dynamic, 1> IndexVector;
00284                 typedef typename Eigen::Matrix<Value, Eigen::Dynamic, 1> ValueVector;
00285                 
00287                 Entries data;
00289                 const VT& headValueRef;
00291                 const size_t sizeMinusOne;
00292                 
00294 
00295                 IndexHeapBruteForceVector(const size_t size):
00296                 data(size, Entry(0, std::numeric_limits<VT>::infinity())),
00297                 headValueRef((data.end() - 1)->value),
00298                 sizeMinusOne(data.size() - 1)
00299                 {
00300                 }
00301                 
00303                 inline void reset()
00304                 {
00305                         for (typename Entries::iterator it(data.begin()); it != data.end(); ++it)
00306                                 it->value = std::numeric_limits<VT>::infinity();
00307                 }
00308                 
00310 
00311                 inline const VT& headValue() const { return headValueRef; }
00312                 
00314 
00316                 inline void replaceHead(const Index index, const Value value)
00317                 {
00318                         register size_t i;
00319                         for (i = sizeMinusOne; i > 0; --i)
00320                         {
00321                                 if (data[i-1].value > value)
00322                                         data[i] = data[i-1];
00323                                 else
00324                                         break;
00325                         }
00326                         data[i].value = value;
00327                         data[i].index = index;
00328                 }
00329                 
00331                 inline void sort()
00332                 {
00333                         // no need to sort as data are already sorted
00334                 }
00335                 
00337 
00339                 template<typename DI, typename DV>
00340                 inline void getData(const Eigen::MatrixBase<DI>& indices, const Eigen::MatrixBase<DV> & values) const
00341                 {
00342                         // note: we must implement this hack because of problem with reference to temporary
00343                         // C++0x will solve this with rvalue
00344                         // see: http://eigen.tuxfamily.org/dox-devel/TopicFunctionTakingEigenTypes.html
00345                         // for more informations
00346                         for (size_t i = 0; i < data.size(); ++i)
00347                         {
00348                                 const_cast<Eigen::MatrixBase<DI>&>(indices).coeffRef(i) = data[i].index;
00349                                 const_cast<Eigen::MatrixBase<DV>&>(values).coeffRef(i) = data[i].value;
00350                         }
00351                 }
00352 #if 0
00353 
00354 
00355                 inline IndexVector getIndexes() const
00356                 {
00357                         IndexVector indexes(data.size());
00358                         for (size_t i = 0; i < data.size(); ++i)
00359                                 indexes.coeffRef(i) = data[i].index;
00360                         return indexes;
00361                 }
00362 #endif
00363         };
00364 }
00365 
00366 #endif // __INDEX_HEAP_H