Incrementally add points to point cloud and perform nearest neighbour search

Hello,

I use an algorithm for 3D path planning that alternately creates a new point, finds the nearest neighbor in an existing point cloud and then adds the new point to the point cloud. This step is being repeated hundreds of thousands of times which means that I need a data structure (k-d tree, octree, space partitioning in general) with which I can quickly find the nearest neighbor of a point and then append the point to it.

I tried to use the pointCloud object but the only way to make it grow is by merging it with another pointCloud which is a very computationally expensive task and thus is not an option for me.

Does anybody know of a Matlab built-in way or FEX contribution to perform nearest neighbor searches on a data structure that can grow incrementally?

Thank you in advance

[Mohsin Shah]

Matlab findNearestNeighbors command for point clouds may be of use to you. There is also another toolbox https://www.geo.tuwien.ac.at/downloads/pg/pctools/pctools.html

[Preetham Manjunatha]

Matlab's knnsearch works well to perform nearest neighbour search. Also this can be used to calculate the cloud-to-cloud distance (c2c distance as in CloudCompare) between two point clouds.

Answer 1

[Sachin Meena]

I believe an approach similar to k-means clustering might work for you - as long as you do not have thousands of clusters. Try the following:

1. Start with some initial points as cluster centers
2. Check distances of new point from the cluster centers, and associate the new point to nearest cluster center
3. Update the cluster center (not computationally extensive)
4. If you wish to retrieve the cluster to which a point belongs, you can setup a lookup table with point as key - you will need to look for a hash function that suits you.

In case you want to increment the number of clusters dynamically, you can set a threshold distance for that. Whenever the nearest neighbor is farther than the threshold value, the new point will start its own cluster.

[Sebastian]

Hello Sachin,

thank you for your efforts. This sounds like an interesting approach to the problem, but I implemented my own Octree with knn search and radius search which works pretty well for me.

[Mohammadreza Yavari]

Hi Sebastian,

I have the exact same problem you solved (find nearest neighbour in RRT). Is it possible for you to share your solution?

[Dave Ober]

Hello Sebastian,

I am not sure if you answered Mohammadreza privately but your Octree with Knn search sounds like a great marriage. Is your solution available to the public? Other researchers?

Thank you