2

I have an array of 3D points called points. Here are the first 20 points of that array:

points = np.array([[-1.33309284,-1.30808319,4.00199986],[-1.33209359,-1.30417236,3.99900007],[-1.33109427,-1.30026584,3.99600005],[-1.35235626,-1.30090892,4.00699997],[-1.34447409,-1.29896096,4.00099993],[-1.33627494,-1.29668835,3.99399996],[-1.35134384,-1.29700102,4.00400019],[-1.34346598,-1.29505737,3.99799991],[-1.33527122,-1.29278989,3.99099994],[-0.43118706,-1.29732492,4.00500011],[-0.42564261,-1.29829663,4.0079999,],[-1.35033125,-1.29309735,4.00099993],[-1.34245787,-1.29115818,3.99499989],[-1.33393295,-1.28857266,3.98699999],[-1.35809791,-1.28919816,3.99799991],[-1.35089223,-1.28790834,3.99399996],[-1.34470857,-1.2875859,3.99300003],[-1.36034349,-1.28562515,3.99600005],[-1.35381569,-1.28498166,3.99399996],[-1.34695627,-1.28401647,3.99099994]])

I have four x,y coordinates, which represent estimated corner points, but they are not necessarily present in the points array due to resolution.

corner_points = np.array([[-1.33109423,-1.30026583],[-1.33527123,-1.29278983],[-1.35089222,-1.28790833],[-1.33393293,-1.28857263]])

I want to find the four points in the points array that are the closest match to the x,y coordinates of the corner points.

Note: It was mentioned that pandas might be a more powerful tool for the task or a ball tree search from scikit i assume. I am not solely bound to using numpy. It would just be practical since I already use it in the script.

Martin Pedersen
  • 153
  • 8
  • `points[0:1]` would be equal to `[[-1.33309284,-1.30808319,4.00199986]]`, that's not what you want to compare to, right? You want to compare the first two elements of each list in the points list to the lists in `corner_points`. Also are you sure there will be exact matches for the corner points? – alexschu98 Aug 11 '22 at 11:20
  • Yes I want to compare the x,y points of all elements in the points list to the x,y points of the corner points list. In the presented case, yes the corner points matches four points in the other list. I assume it involves some numpy slicing, but I can't seem to figure it out – Martin Pedersen Aug 11 '22 at 11:24
  • Are you converting to a numpy array? – mochsner Aug 11 '22 at 11:28
  • I can if needed, but I am still unsure how to structure the where statement to match the first two values of each sub array – Martin Pedersen Aug 11 '22 at 11:29
  • Well, part of the problem (why you couldn't figure it out on your own) may be that your corner_points is not a well formed array. Converting it to an np.array (best practice in numpy) catches that. – mochsner Aug 11 '22 at 11:30
  • I have converted both arrays to numpy arrays, but still no luck – Martin Pedersen Aug 11 '22 at 11:37
  • You cannot safely use equality on FP numbers because this will be wrong if there is any very slight imprecision (see https://stackoverflow.com/questions/588004/is-floating-point-math-broken). A usual way of doing this is to do an approximate equality check with an epsilon (which does not work for big values, in that case a ratio is needed). This is what `np.allclose` does. – Jérôme Richard Aug 11 '22 at 11:50
  • In this specific use-case, I would personally use a ball-tree so to search the closest point to the input one. You can then use the distance so to filter the points that are not found because their distance are too big. The choice of threshold is defined by the exact use-case and its real-world meaning. – Jérôme Richard Aug 11 '22 at 11:52
  • Okay, so you were actually right, the points are not present due to the resolution of my depth data, so I actually need to find the closest x,y points to my corner points, I will edit the question to match – Martin Pedersen Aug 12 '22 at 06:54

2 Answers2

1

First, I agree with Jérôme Richard that it isn't good practice to assume equality between the 2D/3D points (usually, one would "unproject" the 2D points to do a similar thing here).

Here is an approach using numpy. The relations/mapping aspect of numpy isn't as great as pandas, so there may be alternate approaches if you wanted to expand the dependencies into a more relational library such as it (Pandas uses Numpy behind the scenes).

At this time, I can't think of a way to conditionally reduce the mask_2d array into a 1d mask only when both conditions are true (without pandas), but I am sure there is a way. Anyone else, feel free to expand on this answer.

import numpy as np

points = np.array([[-1.33309284,-1.30808319,4.00199986],[-1.33209359,-1.30417236,3.99900007],[-1.33109427,-1.30026584,3.99600005],[-1.35235626,-1.30090892,4.00699997],[-1.34447409,-1.29896096,4.00099993],[-1.33627494,-1.29668835,3.99399996],[-1.35134384,-1.29700102,4.00400019],[-1.34346598,-1.29505737,3.99799991],[-1.33527122,-1.29278989,3.99099994],[-0.43118706,-1.29732492,4.00500011],[-0.42564261,-1.29829663,4.0079999,],[-1.35033125,-1.29309735,4.00099993],[-1.34245787,-1.29115818,3.99499989],[-1.33393295,-1.28857266,3.98699999],[-1.35809791,-1.28919816,3.99799991],[-1.35089223,-1.28790834,3.99399996],[-1.34470857,-1.2875859,3.99300003],[-1.36034349,-1.28562515,3.99600005],[-1.35381569,-1.28498166,3.99399996],[-1.34695627,-1.28401647,3.99099994]])

# corner_points: removed [-1.26,0.48,3.5999999999999996], and fixed too many ] 
corner_points = np.array([-1.33109427,-1.30026584],[-1.33527122,-1.29278989],,[-1.33393295,-1.28857266],[-1.36034349,-1.28562515]]) 

mask_2d = np.isin(points, corner_points)
""" 
Out[92]: 
array(
       [[False, False, False],
       [False, False, False],
       [ True,  True, False], ....
"""

mask_1d = mask[:,0] # Note that this assumes the first and second value  are equal. Possible hole in code.
"""
Out[93]: 
array([False, False,  True, False, False, False, False, False,  True,
   False, False, False, False,  True, False, False, False,  True,
   False, False])
"""

points[mask_1d]
"""
Out[94]: 
array([[-1.33109427, -1.30026584,  3.99600005],
       [-1.33527122, -1.29278989,  3.99099994],
       [-1.33393295, -1.28857266,  3.98699999],
       [-1.36034349, -1.28562515,  3.99600005]])
"""
mochsner
  • 307
  • 2
  • 10
  • 1
    In my case the 2D points are also taken from 3D space, I just wan't to fetch the height of the specific positions because the 3D space is created from dynamic depth data. I will test out your code and accept it when possible – Martin Pedersen Aug 11 '22 at 14:06
1

I found my answer here: Finding index of nearest point in numpy arrays of x and y coordinates

and did some editing to make it fit on my problem.

import scipy
outer_points = [[1.44, 0.48, 0.600* 6],[-1.26, -1.02, 0.600* 6],[-1.26, 0.48, 0.600* 6],[1.44, -1.02, 0.600* 6]]
mytree = scipy.spatial.cKDTree(np.asarray(point_cloud.points))
dist, indexes = mytree.query(outer_points)
points_with_height = [np.asarray(point_cloud.points)[idx] for idx in indexes]

I needed the height of the corner points to do some trigonometry, so when I knew the four points, I had to figure out which is the top left, right and bottom left, right corners.

top_left = [0,0,0]
top_right = [0,0,0]
bottom_left = [0,0,0]
bottom_right = [0,0,0]
for point in points_with_height:
    if point[0] < top_left[0] and point[1] > top_left[1]:
        top_left = point
    if point[0] > top_left[0] and point[1] > top_right[1]:
        top_right = point
    if point[0] < bottom_left[0] and point[1] < bottom_left[1]:
        bottom_left = point
    if point[0] > bottom_right[0] and point[1] < bottom_right[1]:
        bottom_right = point

I don't think it's pretty, but it works. So if anyone has a more elegant way of doing this, then feel free to edit or comment. Thanks for all the help, pointing me in the right direction.

Martin Pedersen
  • 153
  • 8