diff --git a/corelib/src/rtflann/algorithms/kdtree_index.h b/corelib/src/rtflann/algorithms/kdtree_index.h
index 80b9a254..bd3bf62b 100644
--- a/corelib/src/rtflann/algorithms/kdtree_index.h
+++ b/corelib/src/rtflann/algorithms/kdtree_index.h
@@ -47,6 +47,7 @@
 #include "rtflann/util/allocator.h"
 #include "rtflann/util/random.h"
 #include "rtflann/util/saving.h"
+#include "rtabmap/utilite/ULogger.h"
 
 
 namespace rtflann
@@ -141,7 +142,7 @@ public:
     
     void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
     {
-        assert(points.cols==veclen_);
+        UASSERT(points.cols==veclen_);
 
         size_t old_size = size_;
         extendDataset(points);
@@ -664,6 +665,7 @@ private:
     
     void addPointToTree(NodePtr node, int ind)
     {
+    	UDEBUG("ind=%d", ind);
         ElementType* point = points_[ind];
         
         if ((node->child1==NULL) && (node->child2==NULL)) {
diff --git a/corelib/src/rtflann/algorithms/nn_index.h b/corelib/src/rtflann/algorithms/nn_index.h
index 66531d21..2edd9399 100644
--- a/corelib/src/rtflann/algorithms/nn_index.h
+++ b/corelib/src/rtflann/algorithms/nn_index.h
@@ -144,7 +144,9 @@ public:
 	 */
     virtual void buildIndex(const Matrix<ElementType>& dataset)
     {
+    	UDEBUG("");
         setDataset(dataset);
+        UDEBUG("");
         this->buildIndex();
     }
 
@@ -767,6 +769,7 @@ protected:
     void extendDataset(const Matrix<ElementType>& new_points)
     {
     	size_t new_size = size_ + new_points.rows;
+    	UDEBUG("size=%d, new_size=%d, removed_=%d", size_, new_size, removed_?1:0);
     	if (removed_) {
     		removed_points_.resize(new_size);
     		ids_.resize(new_size);
@@ -780,6 +783,7 @@ protected:
     		}
     	}
     	size_ = new_size;
+    	UDEBUG("last_id_=%d", last_id_);
     }