pythonのnp.array配列に格納した２次元座標から、入力した座標に最も近い座標を取得した際の備忘録です

目的

pythonのnp.array配列に格納した２次元座標から、入力した座標に最も近い座標を取得した際の備忘録です

コード

sample.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import numpy as np

PI = 3.1415
coordinate = np.array([
        # x, y, th
        [0.5, 0, PI],
        [0.9, 0, PI],
        [0.9, -0.5, PI],
        [0.9, 0.5, PI],
        [0.9, 0, PI],
        [0, 0.5, -1*PI/2],
        [0, 0.5, 0],
        [0, 0.5, PI],
        [-0.5, 0, 0],
        [-0.9, 0, 0],
        [-0.9, 0.5, 0],
        [-0.9, -0.5, 0],
        [-0.9, 0, 0],
        [0, -0.5, PI/2],
        [0, -0.5, 0],
        [0, -0.5, PI],
        [0, -0.5, PI/4]
])

# 点p0に一番近い点を取得
def func_search_neighbourhood(p0, ps):
    L = np.array([])
    for i in range(ps.shape[0]):
        norm = np.sqrt( (ps[i][0] - p0[0])*(ps[i][0] - p0[0]) +
                        (ps[i][1] - p0[1])*(ps[i][1] - p0[1]) )
        L = np.append(L, norm)
    return np.argmin(L) ,ps[np.argmin(L)]


def main():
    # target coordinate
    _x = 0
    _y = -0.7

    target_coor = np.array([_x, _y])
    idx, nearrest_coor = func_search_neighbourhood(target_coor, coordinate)
    print( len( coordinate ) )
    print( idx, nearrest_coor[0], nearrest_coor[1], nearrest_coor[2] ) # idx, (x, y, th)

if __name__ == '__main__':
    main()

実行結果

$ python sample.py
17
13 0.0 -0.5 1.57075

参考

Python/NumPyで2点間の距離を計算
辞書内のPythonの最も近い座標
Python/Numpyで最も近い点を探索
Finding the closest point to a list of points