KDTreeSingleIndexDynamicAdaptor is mush slower than KDTreeSingleIndexAdaptor

[getupgetup]

Hi, I'm using nanoflann to construct a class called KdTreeFLANN in order to replace pcl::KdTreeFLANN. It is almost the same as the practice in https://github.com/laboshinl/loam_velodyne. It is based on nanoflann::KDTreeSingleIndexAdaptor, so it can't add/remove points dynamicly. Then I noticed nanoflann::KDTreeSingleIndexDynamicAdaptor which support add/remove points.So I change my class to support adding points dynamicly by nanoflann::KDTreeSingleIndexDynamicAdaptor. However, I found that it consumes about 2.5x time compared with the one before changing using the same point cloud data. Is it common or am I missing something or using anying wrongly?

[getupgetup]

Here is my code:
`
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/shared_ptr.hpp>
#include "nanoflann.hpp"

namespace nanoflann {

template
class PointCloudAdaptor {
public:
PointCloudAdaptor() { point_cloud_.reset(new PointCloud); }

private:
using PointCloud = typename pcl::PointCloud;
using PointCloudPtr = typename pcl::PointCloud::Ptr;
using PointCloudConstPtr = typename pcl::PointCloud::ConstPtr;

PointCloudConstPtr point_cloud_;

public:
void setInputCloud(const PointCloudPtr& cloud) { point_cloud_ = cloud; }

void addNewCloud(const PointCloudPtr& new_cloud) {
*point_cloud_ += *new_cloud;
}

inline size_t kdtree_get_point_count() const {
return point_cloud_->points.size();
}

inline float kdtree_get_pt(const size_t idx, int dim) const {
const PointT& p = point_cloud_->points[idx];
if (dim == 0) {
return p.x;
} else if (dim == 1) {
return p.y;
} else if (dim == 2) {
return p.z;
}
return 0.0;
}

template
bool kdtree_get_bbox(BBOX&) const {
return false;
}
};

template
class DynamicKdTreeFLANN {
public:
explicit DynamicKdTreeFLANN(bool sorted = true) : kdtree_(3, adaptor_) {
params_.sorted = sorted;
}

DynamicKdTreeFLANN(const DynamicKdTreeFLANN& rhs) = delete;
DynamicKdTreeFLANN& operator=(const DynamicKdTreeFLANN& rhs) = delete;

private:
using KDTreeFlann_PCL_SO3 = KDTreeSingleIndexDynamicAdaptor<
SO3_Adaptor<float, PointCloudAdaptor>, PointCloudAdaptor,
3>;

SearchParams params_;
PointCloudAdaptor adaptor_;
KDTreeFlann_PCL_SO3 kdtree_;

public:
typedef boost::shared_ptr<DynamicKdTreeFLANN> Ptr;
typedef boost::shared_ptr<const DynamicKdTreeFLANN> ConstPtr;

typedef typename pcl::PointCloud PointCloud;
typedef typename pcl::PointCloud::Ptr PointCloudPtr;
typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr;

inline Ptr makeShared() { return Ptr(new DynamicKdTreeFLANN(*this)); }

void setEpsilon(float eps) { params_.eps = eps; }

void setSortedResults(bool sorted) { params_.sorted = sorted; }

void setInputCloud(const PointCloudPtr& cloud) {
adaptor_.setInputCloud(cloud);
CHECK_GT(adaptor_.kdtree_get_point_count(), 0);
kdtree_.addPoints(0, adaptor_.kdtree_get_point_count() - 1);
}

void AddNewCloud(const PointCloudPtr& new_cloud) {
const auto size = adaptor_.kdtree_get_point_count();
adaptor_.addNewCloud(new_cloud);
CHECK_GT(new_cloud->size(), 0);
kdtree_.addPoints(size, adaptor_.kdtree_get_point_count() - 1);
}

int nearestKSearch(const PointT& point, int num_closest,
std::vector& k_indices,
std::vector& k_sqr_distances) const {
k_indices.resize(num_closest);
k_sqr_distances.resize(num_closest);
nanoflann::KNNResultSet<float, int> resultSet(num_closest);
resultSet.init(k_indices.data(), k_sqr_distances.data());
kdtree_.findNeighbors(resultSet, point.data, nanoflann::SearchParams());
return resultSet.size();
}

int radiusSearch(const PointT& point, double radius,
std::vector& k_indices,
std::vector& k_sqr_distances) const {
static std::vector<std::pair<int, float>> indices_dist;
indices_dist.reserve(128);
RadiusResultSet<float, int> resultSet(radius, indices_dist);
const size_t nFound = kdtree_.findNeighbors(resultSet, point.data, params_);
if (params_.sorted)
std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter());
k_indices.resize(nFound);
k_sqr_distances.resize(nFound);
for (int i = 0; i < nFound; i++) {
k_indices[i] = indices_dist[i].first;
k_sqr_distances[i] = indices_dist[i].second;
}
return nFound;
}
};

} // namespace nanoflann
`

[stan-guer]

Did you ever come closer to a possible solution to the problem?
This seems to be related to #90

[getupgetup]

Did you ever come closer to a possible solution to the problem?
This seems to be related to #90

Nope, I've accept this fact. I think if you need to add points frequently, KDTreeSingleIndexDynamicAdaptor is better.If not, KDTreeSingleIndexAdaptor is faster.

[ibtaylor]

From what I can tell, instead of using std::vector<int> for treeIndex, one could use it more as a mask. If we make it std::vector<int8_t> instead, and rather than storing pos, just maintain say -1,1 or whatever values to mark removed points, then it should behave the same but with 4x effective usage of L1/L2/L3 caches. Nowhere in the code did I see usage of treeIndex values other than checking against -1.

Thoughts on this change?

[getupgetup]

should behave the same but with 4x eff

Thank you for your reply. I'm sorry that I don't understand the internal principal of KDTreeSingleIndexDynamicAdaptor clearly. I just use it as https://github.com/jlblancoc/nanoflann/blob/master/examples/dynamic_pointcloud_example.cpp
So if I don't store pos, how can I visit the point of specific pos/index? Would you please give me an example~ Thank you~

[cynosure4sure]

@getupgetup Facing the same issue for large 3D pointclouds. Did you resolve this with nanoflann or moved to another library? Any help would be appreciated.
Thank you.

[getupgetup]

@getupgetup Facing the same issue for large 3D pointclouds. Did you resolve this with nanoflann or moved to another library? Any help would be appreciated.
Thank you.

Well, it seems that KDTreeSingleIndexDynamicAdaptor is slower than KDTreeSingleIndexAdaptor. Fortunately for me, kdtree is updated frequently so that KDTreeSingleIndexDynamicAdaptor is much better than KDTreeSingleIndexAdaptor. PS: I tried to modifiy the code by @ibtaylor 's advice but it seemed not very useful. The efficiency increased by only 1-2%.

[ibtaylor]

@getupgetup can you put together a gist with simple cases that shows the 2.5x slowdown?

[cynosure4sure]

Well, it seems that KDTreeSingleIndexDynamicAdaptor is slower than KDTreeSingleIndexAdaptor. Fortunately for me, kdtree is updated frequently so that KDTreeSingleIndexDynamicAdaptor is much better than KDTreeSingleIndexAdaptor.

@getupgetup Thank you for the quick reply.
Can you kindly comment at what rate/frequency you are updating your kdtree at and what is the average size of points added/removed in each update?

[getupgetup]

Well, it seems that KDTreeSingleIndexDynamicAdaptor is slower than KDTreeSingleIndexAdaptor. Fortunately for me, kdtree is updated frequently so that KDTreeSingleIndexDynamicAdaptor is much better than KDTreeSingleIndexAdaptor.

@getupgetup Thank you for the quick reply.
Can you kindly comment at what rate/frequency you are updating your kdtree at and what is the average size of points added/removed in each update?

It has been a long time since my project finished. I used KDTreeSingleIndexDynamicAdaptor to update my map for localizing(ICP match). The update frequecy is about 10Hz and the average size of points updated is about 10e3。

[getupgetup]

@getupgetup can you put together a gist with simple cases that shows the 2.5x slowdown?

It has been a long time since my project finished. I need to take some time to clean up my code if I'm free.

[dschwen]

See #139

[getupgetup]

Great! Thank you!