35 #ifndef RTABMAP_FLANN_LSH_TABLE_H_ 36 #define RTABMAP_FLANN_LSH_TABLE_H_ 43 #if RTFLANN_USE_UNORDERED_MAP 44 #include <unordered_map> 71 typedef std::vector<FeatureIndex>
Bucket;
99 out <<
"Lsh Table Stats:\n" << std::setw(w) << std::setiosflags(std::ios::right) <<
"N buckets : " 100 << stats.
n_buckets_ <<
"\n" << std::setw(w) << std::setiosflags(std::ios::right) <<
"mean size : " 101 << std::setiosflags(std::ios::left) << stats.
bucket_size_mean_ <<
"\n" << std::setw(w)
102 << std::setiosflags(std::ios::right) <<
"median size : " << stats.
bucket_size_median_ <<
"\n" << std::setw(w)
103 << std::setiosflags(std::ios::right) <<
"min size : " << std::setiosflags(std::ios::left)
104 << stats.
bucket_size_min_ <<
"\n" << std::setw(w) << std::setiosflags(std::ios::right) <<
"max size : " 108 out << std::endl << std::setw(w) << std::setiosflags(std::ios::right) <<
"histogram : " 109 << std::setiosflags(std::ios::left);
110 for (std::vector<std::vector<unsigned int> >::const_iterator iterator = stats.
size_histogram_.begin(), end =
111 stats.
size_histogram_.end(); iterator != end; ++iterator) out << (*iterator)[0] <<
"-" << (*iterator)[1] <<
": " << (*iterator)[2] <<
", ";
124 template<
typename ElementType>
130 #if RTFLANN_USE_UNORDERED_MAP 131 typedef std::unordered_map<BucketKey, Bucket>
BucketsSpace;
153 std::cerr <<
"LSH is not implemented for that type" << std::endl;
161 void add(
unsigned int value,
const ElementType* feature)
164 BucketKey key = getKey(feature);
166 switch (speed_level_) {
169 buckets_speed_[key].push_back(value);
173 key_bitset_.set(key);
174 buckets_space_[key].push_back(value);
179 buckets_space_[key].push_back(value);
188 void add(
const std::vector< std::pair<size_t, ElementType*> >& features)
190 #if RTFLANN_USE_UNORDERED_MAP 191 buckets_space_.rehash((buckets_space_.size() + features.size()) * 1.2);
194 for (
size_t i = 0; i < features.size(); ++i) {
195 add(features[i].first, features[i].second);
208 switch (speed_level_) {
211 return &buckets_speed_[key];
215 if (key_bitset_.test(key))
return &buckets_space_.find(key)->second;
221 BucketsSpace::const_iterator bucket_it, bucket_end = buckets_space_.end();
222 bucket_it = buckets_space_.find(key);
224 if (bucket_it == bucket_end)
return 0;
225 else return &bucket_it->second;
236 std::cerr <<
"LSH is not implemented for that type" << std::endl;
254 kArray, kBitsetHash, kHash
261 speed_level_ = kHash;
262 key_size_ = key_size;
270 if (speed_level_ == kArray)
return;
273 if (buckets_space_.size() > ((size_t(1) << key_size_) / 2)) {
274 speed_level_ = kArray;
276 buckets_speed_.resize(
size_t(1) << key_size_);
277 for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) buckets_speed_[key_bucket->first] = key_bucket->second;
280 buckets_space_.clear();
286 if (((
std::max(buckets_space_.size(), buckets_speed_.size()) * CHAR_BIT * 3 *
sizeof(BucketKey)) / 10
287 >=
size_t(
size_t(1) << key_size_)) || (key_size_ <= 32)) {
288 speed_level_ = kBitsetHash;
289 key_bitset_.resize(
size_t(1) << key_size_);
292 for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) key_bitset_.set(key_bucket->first);
295 speed_level_ = kHash;
300 template<
typename Archive>
304 if (Archive::is_saving::value) {
305 val = (int)speed_level_;
308 if (Archive::is_loading::value) {
315 if (speed_level_==kArray) {
318 if (speed_level_==kBitsetHash || speed_level_==kHash) {
321 if (speed_level_==kBitsetHash) {
362 mask_ = std::vector<size_t>((size_t)
ceil((
float)(feature_size *
sizeof(char)) / (float)
sizeof(
size_t)), 0);
365 std::vector<size_t> indices(feature_size * CHAR_BIT);
366 for (
size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
367 std::random_shuffle(indices.begin(), indices.end());
370 for (
unsigned int i = 0; i < key_size_; ++i) {
371 size_t index = indices[i];
374 size_t divisor = CHAR_BIT *
sizeof(size_t);
375 size_t idx = index / divisor;
376 mask_[idx] |= size_t(1) << (index % divisor);
383 BOOST_FOREACH(
size_t mask_block, mask_){
384 out << std::setw(
sizeof(
size_t) * CHAR_BIT / 4) << std::setfill(
'0') << std::hex << mask_block
386 bcount += __builtin_popcountll(mask_block);
388 out <<
"bit count : " << std::dec << bcount << std::endl;
389 out <<
"mask size : " << mask_.size() << std::endl;
403 const size_t* feature_block_ptr =
reinterpret_cast<const size_t*
> (feature);
408 size_t subsignature = 0;
409 size_t bit_index = 1;
411 for (std::vector<size_t>::const_iterator pmask_block = mask_.begin(); pmask_block != mask_.end(); ++pmask_block) {
413 size_t feature_block = *feature_block_ptr;
414 size_t mask_block = *pmask_block;
417 size_t lowest_bit = mask_block & (-(ptrdiff_t)mask_block);
419 subsignature += (feature_block & lowest_bit) ? bit_index : 0;
421 mask_block ^= lowest_bit;
436 if ((buckets_speed_.empty()) && (buckets_space_.empty())) {
444 if (!buckets_speed_.empty()) {
445 for (BucketsSpeed::const_iterator pbucket = buckets_speed_.begin(); pbucket != buckets_speed_.end(); ++pbucket) {
453 for (BucketsSpace::const_iterator x = buckets_space_.begin(); x != buckets_space_.end(); ++x) {
476 unsigned int bin_start = 0;
477 unsigned int bin_end = 20;
478 bool is_new_bin =
true;
481 if (*iterator < bin_end) {
size_t getKey(const ElementType *) const
ROSCONSOLE_DECL void initialize()
std::ostream & operator<<(std::ostream &out, const LshStats &stats)
void add(unsigned int value, const ElementType *feature)
std::vector< unsigned int > bucket_sizes_
BucketsSpace buckets_space_
const Bucket * getBucketFromKey(BucketKey key) const
std::vector< std::vector< unsigned int > > size_histogram_
LshTable(unsigned int, unsigned int)
size_t bucket_size_median_
void add(const std::vector< std::pair< size_t, ElementType * > > &features)
LshStats getStats() const
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
void serialize(Archive &ar)
DynamicBitset key_bitset_
std::map< BucketKey, Bucket > BucketsSpace
size_t bucket_size_std_dev
std::vector< size_t > mask_
std::vector< FeatureIndex > Bucket
void initialize(size_t key_size)
std::vector< Bucket > BucketsSpeed
GLM_FUNC_DECL genType ceil(genType const &x)
BucketsSpeed buckets_speed_