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

Added Debug log in FLANN kd-trees index

Browse Source
This commit is contained in:
matlabbe
2015-10-14 13:26:28 -04:00
parent 457e1e9d21
commit 2f5d41fb3c
2 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -47,6 +47,7 @@
#include "rtflann/util/allocator.h" #include "rtflann/util/allocator.h"
#include "rtflann/util/random.h" #include "rtflann/util/random.h"
#include "rtflann/util/saving.h" #include "rtflann/util/saving.h"
#include "rtabmap/utilite/ULogger.h"
namespace rtflann namespace rtflann
@@ -141,7 +142,7 @@ public:
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2) void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{ {
assert(points.cols==veclen_); UASSERT(points.cols==veclen_);
size_t old_size = size_; size_t old_size = size_;
extendDataset(points); extendDataset(points);
@@ -664,6 +665,7 @@ private:
void addPointToTree(NodePtr node, int ind) void addPointToTree(NodePtr node, int ind)
{ {
UDEBUG("ind=%d", ind);
ElementType* point = points_[ind]; ElementType* point = points_[ind];
if ((node->child1==NULL) && (node->child2==NULL)) { if ((node->child1==NULL) && (node->child2==NULL)) {

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

@@ -144,7 +144,9 @@ public:
*/ */
virtual void buildIndex(const Matrix<ElementType>& dataset) virtual void buildIndex(const Matrix<ElementType>& dataset)
{ {
UDEBUG("");
setDataset(dataset); setDataset(dataset);
UDEBUG("");
this->buildIndex(); this->buildIndex();
} }
@@ -767,6 +769,7 @@ protected:
void extendDataset(const Matrix<ElementType>& new_points) void extendDataset(const Matrix<ElementType>& new_points)
{ {
size_t new_size = size_ + new_points.rows; size_t new_size = size_ + new_points.rows;
UDEBUG("size=%d, new_size=%d, removed_=%d", size_, new_size, removed_?1:0);
if (removed_) { if (removed_) {
removed_points_.resize(new_size); removed_points_.resize(new_size);
ids_.resize(new_size); ids_.resize(new_size);
@@ -780,6 +783,7 @@ protected:
} }
} }
size_ = new_size; size_ = new_size;
UDEBUG("last_id_=%d", last_id_);
} }