0.12.3: Increased Tango rendering performance, modified all handleEvent() with new interface (now returning bool)

corelib/src/rtflann/util/heap.h

@@ -33,6 +33,7 @@
 
 #include <algorithm>
 #include <vector>
+#include <rtabmap/utilite/ULogger.h>
 
 namespace rtflann
 {
@@ -86,6 +87,15 @@ public:
         return count;
     }
 
+    /**
+	 *
+	 * Returns: heap size
+	 */
+	int capacity()
+	{
+		return length;
+	}
+
     /**
      * Tests if the heap is empty
      *
@@ -129,6 +139,7 @@ public:
             return;
         }
 
+        UASSERT(heap.size() < heap.capacity());
         heap.push_back(value);
         static CompareT compareT;
         std::push_heap(heap.begin(), heap.end(), compareT);