libnabo
1.0.4
|
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