nearest_neighbor.py

class timer:
    def __init__(self, msg, fmt="%0.3g"):
        self.msg = msg
        self.fmt = fmt

    def __enter__(self):
        self.start = time.process_time()
        return self

    def __exit__(self, *args):
        t = time.process_time() - self.start
        log.info(("%s : " + self.fmt + " seconds") % (self.msg, t))
        self.time = t
        
vehicle_positions = np.random.normal(size=(28, 3))

with timer('kd tree total'):
    with timer('kd tree build'):
        kd_tree = scipy.spatial.cKDTree(vehicle_positions)

    ego_position = [8000.45654297, 27470.125, 24762.65039062]

    with timer('kd tree query'):
        distance, index = kd_tree.query(ego_position)

with timer('brute force'):
    min_dist = None
    min_index = -1
    for index, point in enumerate(vehicle_positions):
        dist = scipy.spatial.distance.cdist(
            np.array([point]), np.array([ego_position]))[0][0]
        if min_dist is None or dist < min_dist:
            min_dist = dist
            min_index = index
    distance = min_dist
    index = min_index
    
"""
INFO:__main__:kd tree build : 6.57e-05 seconds
INFO:__main__:kd tree query : 7.55e-05 seconds
INFO:__main__:kd tree total : 0.000376 seconds
INFO:__main__:brute force : 0.000393 seconds
"""

kd tree build : 6.16e-05 seconds
kd tree query : 7.05e-05 seconds
kd tree total : 0.000158 seconds
brute force : 0.000171 seconds