Finding closest x,y,z points from x,y coordinates-CodePudding

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.

CodePudding user response：

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]])
"""

CodePudding user response：

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.