Select all points in the matrix within 30 m from another point

Question

So, if you look at my other posts, it is not surprising that I create a robot that can collect data in the forest and bind it to the map. We have algorithms that can determine the centers of trees and trunk diameters and can insert them into the Cartesian XY plane.

We plan to use certain “key” trees as natural landmarks for robot localization using triangulation and trilateration among other methods, but programming this and saving data directly and effectively makes it difficult to use only Matlab.

Is there a method for substituting an array or matrix of points? Let's say I have 1000 trees stored at a distance of more than 1 km (1000 m), is there any way to say, select only points within a radius of 30 m from my current location and work only with them?

I would just use GIS, but I do it in Matlab, and I don't know about any GIS plugins for Matlab.

I forgot to mention that this code goes online, which means that it is launched by the robot to execute in real time. I don’t know if, when the map grows to several miles, using a different data structure will help or if calculating each distance to a random point is what the spatial database will do anyway.

I think about mirroring an array of trees into two arrays, one of which is sorted by X, and the other by Y. Then sort the bubbles to determine the range of 30 m in this. I do the same for both arrays, X and Y, and then have a third cross-reference table that will select individual values. But I don’t know what it's called, how to program it, and I’m sure that someone already exists, so I don’t want to reinvent the wheel.

Cartesian Plan
GIS

Answer 1

A simple solution for calculating all distances and scanning seems to work almost instantly:

lim = 1; num_trees = 1000; trees = randn(num_trees,2); %# list of trees as Nx2 matrix cur = randn(1,2); %# current point as 1x2 vector dists = hypot(trees(:,1) - cur(1), trees(:,2) - cur(2)); %# distance from all trees to current point nearby = tree_ary((dists <= lim),:); %# find the nearby trees, pull them from the original matrix 

On a 1.2 GHz machine, I can process 1 million trees (1 MTree?) In <0.4 seconds.

Do you use Matlab code directly on the robot? Are you using Real-time Workshop or something like that? If you need to translate this to C, you can replace hypot with sqr(trees[i].x - pos.x) + sqr(trees[i].y - pos.y) and replace the limit check with < lim^2 . If you really need to deal with 1 KTree, I don’t know what it takes to spend time creating a more complex data structure.

Answer 2

You are looking for a spatial database such as quadtree or kd-tree . I found two implementations of the kd tree here and here , but did not find any quadrant implementations for Matlab.

Answer 3

You can convert your Cartesian coordinates to polar coordinates using CART2POL . Then the selection of points within a certain radius will be scrolled forward.

 [THETA,RHO] = cart2pol(X-X0,Y-Y0); selected = RHO < 30; 

where X0, Y0 are the coordinates of the current location.

Answer 4

I assume that the trees are distributed approximately evenly throughout the forest. If so, just use the 30x30 (or 15x15) lattice blocks as hash keys in the private hash table . Find the keys for all blocks intersecting the search circle, and check all hash entries starting with this key until one of them is marked as the last in its bucket.

 0---------10---------20---------30--------40---------50----- address # line (0,0) (0,30) (0,60) (30,0) (30,30) (30,60) hash key values (1,3) (10,15) (3,46) (24,9.) (23,65.) (15,55.) tree coordinates + "." flag 

For example, to get trees at (0,0) ... (30,30), compare (0,0) with address 0 and read the entries (1,3), (10,15), reject (3,46) , because it goes beyond, read (24.9) and stop because it is marked as the last tree in this sector.

To get the trees at (0.60) ... (30.90), match (0.60) with address 20. Skip (24, 9), read (23, 65) and stop as you last.

This will be quite efficient in terms of memory, since it avoids storing pointers that would otherwise be of considerable size compared to the actual data. However, private hashing requires some space left.

The illustration does not “scale”, since there will actually be space for several entries between the hash key tokens. Therefore, you do not need to skip any entries if there are more trees in the previous local sector than the average.

This uses hash collisions to your advantage, so it is not as random as a hash function. (Not every record corresponds to a clear hash value.) However, since dense areas of the forest will often be adjacent, you must randomize the mapping of sectors to "buckets", so this dense sector, we hope, will overflow into a less dense one, or the next, or the next.

In addition, there is the problem of empty sectors and the completion of the iteration. You can insert a dummy tree in each sector to mark it as empty or some other simple hack.

Sorry for the long explanation. Such things are easier to implement than to document. But productivity and floor space can be great.

Answer 5

Use some structure of spatially separated data. A simple solution is to simply create a 2d array of lists containing all objects in the 30m x 30m area. In the worst case scenario, you only need to compare the objects in four of these lists.

A lot of more complex (and possibly useful) solutions could be used - something like bi-trees is a little more difficult to implement (but not very), but it can get more optimal performance (especially in cases where the density of objects varies significantly).

Answer 6

You can see the voronoi diagram support in matlab:

http://www.mathworks.com/access/helpdesk/help/techdoc/ref/voronoi.html

If you base voronoi polygons on your key trees and put adjacent trees in these polygons, this will split your search space into proximity (quickly find a closed polygon for a given non-key point), but ultimately you are "going to switch to a computational key on Implicit distances using Pythagoras or Trigger and compare them.

For several thousand points (trees), brute force can be fast enough if you have a reasonable processor on board. Calculate the distance of any other tree from tree n, then select those that are within 30 '. This is the same as all the trees in the same raven polygon.

It has been several years since I worked in GIS, but I found the following useful: "Computational geometry in C" Joseph O Rourke, ISBN 0-521-44592-2 Paperback.