i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Debug log in incremental FLANN stuff (to track a crashing bug)

Browse Source
This commit is contained in:
matlabbe
2015-10-14 11:03:43 -04:00
parent 9bd649009c
commit 457e1e9d21
3 changed files with 24 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
corelib/src/VWDictionary.cpp
View File

@@ -499,14 +499,20 @@ void VWDictionary::update()
_strategy < kNNBruteForce && _strategy < kNNBruteForce &&
_visualWords.size()) _visualWords.size())
{ {
ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter) for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
{ {
UASSERT(uContains(_mapIdIndex, *iter)); UASSERT(uContains(_mapIdIndex, *iter));
UASSERT(uContains(_mapIndexId, _mapIdIndex.at(*iter)));
_flannIndex->removePoint(_mapIdIndex.at(*iter)); _flannIndex->removePoint(_mapIdIndex.at(*iter));
_mapIndexId.erase(_mapIdIndex.at(*iter));
_mapIdIndex.erase(*iter);
} }
ULOGGER_DEBUG("Incremental FLANN: Removing %d words... done!", (int)_removedIndexedWords.size());
if(_notIndexedWords.size()) if(_notIndexedWords.size())
{ {
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter) for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{ {
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0); VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -514,6 +520,7 @@ void VWDictionary::update()
int index = 0; int index = 0;
if(!_flannIndex->isBuilt()) if(!_flannIndex->isBuilt())
{ {
UDEBUG("Building FLANN index...");
switch(_strategy) switch(_strategy)
{ {
case kNNFlannNaive: case kNNFlannNaive:
@@ -531,6 +538,7 @@ void VWDictionary::update()
UFATAL("Not supposed to be here!"); UFATAL("Not supposed to be here!");
break; break;
} }
UDEBUG("Building FLANN index... done!");
} }
else else
{ {
@@ -538,14 +546,13 @@ void VWDictionary::update()
UASSERT(w->getDescriptor().type() == _flannIndex->featuresType()); UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
index = _flannIndex->addPoint(w->getDescriptor()); index = _flannIndex->addPoint(w->getDescriptor());
} }
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(index, w->id())); std::pair<std::map<int, int>::iterator, bool> inserted;
std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index)); inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
if(!inserted.second) UASSERT(inserted.second);
{ inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
//update to new index UASSERT(inserted.second);
inserted.first->second = index;
}
} }
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
} }
} }
else if(_strategy >= kNNBruteForce && else if(_strategy >= kNNBruteForce &&
@@ -633,6 +640,7 @@ void VWDictionary::update()
} }
_notIndexedWords.clear(); _notIndexedWords.clear();
_removedIndexedWords.clear(); _removedIndexedWords.clear();
UDEBUG("");
} }
void VWDictionary::clear() void VWDictionary::clear()

5
corelib/src/rtflann/algorithms/kdtree_index.h
View File

@@ -252,12 +252,13 @@ protected:
*/ */
void buildIndexImpl() void buildIndexImpl()
{ {
UDEBUG("");
// Create a permutable array of indices to the input vectors. // Create a permutable array of indices to the input vectors.
std::vector<int> ind(size_); std::vector<int> ind(size_);
for (size_t i = 0; i < size_; ++i) { for (size_t i = 0; i < size_; ++i) {
ind[i] = int(i); ind[i] = int(i);
} }
UDEBUG("");
mean_ = new DistanceType[veclen_]; mean_ = new DistanceType[veclen_];
var_ = new DistanceType[veclen_]; var_ = new DistanceType[veclen_];
@@ -268,8 +269,10 @@ protected:
std::random_shuffle(ind.begin(), ind.end()); std::random_shuffle(ind.begin(), ind.end());
tree_roots_[i] = divideTree(&ind[0], int(size_) ); tree_roots_[i] = divideTree(&ind[0], int(size_) );
} }
UDEBUG("");
delete[] mean_; delete[] mean_;
delete[] var_; delete[] var_;
UDEBUG("");
} }
void freeIndex() void freeIndex()

6
corelib/src/rtflann/algorithms/nn_index.h
View File

@@ -39,6 +39,7 @@
#include "rtflann/util/result_set.h" #include "rtflann/util/result_set.h"
#include "rtflann/util/dynamic_bitset.h" #include "rtflann/util/dynamic_bitset.h"
#include "rtflann/util/saving.h" #include "rtflann/util/saving.h"
#include "rtabmap/utilite/ULogger.h"
namespace rtflann namespace rtflann
{ {
@@ -124,12 +125,15 @@ public:
*/ */
virtual void buildIndex() virtual void buildIndex()
{ {
UDEBUG("");
freeIndex(); freeIndex();
UDEBUG("");
cleanRemovedPoints(); cleanRemovedPoints();
UDEBUG("");
// building index // building index
buildIndexImpl(); buildIndexImpl();
UDEBUG("");
size_at_build_ = size_; size_at_build_ = size_;
} }