diff --git a/corelib/src/VWDictionary.cpp b/corelib/src/VWDictionary.cpp
index 243f0583..6b59ae35 100644
--- a/corelib/src/VWDictionary.cpp
+++ b/corelib/src/VWDictionary.cpp
@@ -499,14 +499,20 @@ void VWDictionary::update()
 		   _strategy < kNNBruteForce &&
 		   _visualWords.size())
 		{
+			ULOGGER_DEBUG("Incremental FLANN: Removing %d words...", (int)_removedIndexedWords.size());
 			for(std::set<int>::iterator iter=_removedIndexedWords.begin(); iter!=_removedIndexedWords.end(); ++iter)
 			{
 				UASSERT(uContains(_mapIdIndex, *iter));
+				UASSERT(uContains(_mapIndexId, _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())
 			{
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size());
 				for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
 				{
 					VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -514,6 +520,7 @@ void VWDictionary::update()
 					int index = 0;
 					if(!_flannIndex->isBuilt())
 					{
+						UDEBUG("Building FLANN index...");
 						switch(_strategy)
 						{
 						case kNNFlannNaive:
@@ -531,6 +538,7 @@ void VWDictionary::update()
 							UFATAL("Not supposed to be here!");
 							break;
 						}
+						UDEBUG("Building FLANN index... done!");
 					}
 					else
 					{
@@ -538,14 +546,13 @@ void VWDictionary::update()
 						UASSERT(w->getDescriptor().type() == _flannIndex->featuresType());
 						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 = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
-					if(!inserted.second)
-					{
-						//update to new index
-						inserted.first->second = index;
-					}
+					std::pair<std::map<int, int>::iterator, bool> inserted;
+					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
+					UASSERT(inserted.second);
+					inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), index));
+					UASSERT(inserted.second);
 				}
+				ULOGGER_DEBUG("Incremental FLANN: Inserting %d words... done!", (int)_notIndexedWords.size());
 			}
 		}
 		else if(_strategy >= kNNBruteForce &&
@@ -633,6 +640,7 @@ void VWDictionary::update()
 	}
 	_notIndexedWords.clear();
 	_removedIndexedWords.clear();
+	UDEBUG("");
 }
 
 void VWDictionary::clear()
diff --git a/corelib/src/rtflann/algorithms/kdtree_index.h b/corelib/src/rtflann/algorithms/kdtree_index.h
index 105287ae..80b9a254 100644
--- a/corelib/src/rtflann/algorithms/kdtree_index.h
+++ b/corelib/src/rtflann/algorithms/kdtree_index.h
@@ -252,12 +252,13 @@ protected:
      */
     void buildIndexImpl()
     {
+    	UDEBUG("");
         // Create a permutable array of indices to the input vectors.
     	std::vector<int> ind(size_);
         for (size_t i = 0; i < size_; ++i) {
             ind[i] = int(i);
         }
-
+        UDEBUG("");
         mean_ = new DistanceType[veclen_];
         var_ = new DistanceType[veclen_];
 
@@ -268,8 +269,10 @@ protected:
             std::random_shuffle(ind.begin(), ind.end());
             tree_roots_[i] = divideTree(&ind[0], int(size_) );
         }
+        UDEBUG("");
         delete[] mean_;
         delete[] var_;
+        UDEBUG("");
     }
 
     void freeIndex()
diff --git a/corelib/src/rtflann/algorithms/nn_index.h b/corelib/src/rtflann/algorithms/nn_index.h
index bd1e1c70..66531d21 100644
--- a/corelib/src/rtflann/algorithms/nn_index.h
+++ b/corelib/src/rtflann/algorithms/nn_index.h
@@ -39,6 +39,7 @@
 #include "rtflann/util/result_set.h"
 #include "rtflann/util/dynamic_bitset.h"
 #include "rtflann/util/saving.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtflann
 {
@@ -124,12 +125,15 @@ public:
 	 */
 	virtual void buildIndex()
 	{
+		UDEBUG("");
     	freeIndex();
+    	UDEBUG("");
     	cleanRemovedPoints();
+    	UDEBUG("");
 
     	// building index
 		buildIndexImpl();
-
+		UDEBUG("");
         size_at_build_ = size_;
 
 	}