MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

ロボティクスにおける最近傍点探索のためのscipy.spatial.cKDTree入門


NumPy&SciPy数値計算実装ハンドブック (Pythonライブラリ定番セレクション)

目次

はじめに

ロボティクスにおいて、

最近傍点探索はよく使われる計算です。

例えば、衝突検知や、センサ情報の対応点探索などに

最近傍点探索は使われます。

 

最近傍点探索を使う時に、

一番よく問題になるのが、計算量です。

単純にbrute forceで最近傍点を探すと、

計算時間は探索する点の数に応じて増加してしまいます。

 

そこで、よく使われるのがkd-treeです。

en.wikipedia.org

このkd-treeを使うことで、大量のデータがあっても、

比較的、高速に最近傍点探索を計算することができます。

 

このkd-treeを使うには、

様々なライブラリがすでに存在しています.

例えば、以前紹介したFLANNも

kd-treeをデータ構造に利用しています。

myenigma.hatenablog.com

 

今回の記事では、

Pythonの科学技術計算のライブラリであるSciPyの中にある、

scipy.spatial.KDTreeというモジュールの使い方を紹介したいと思います。

scipy.github.io

  

kdtreeとは

kdtreeは、(k dimensional tree)

Tree型のデータ構造の一つで、

複数の次元のデータ点を格納できます。

多次元のデータを二分木で分類するため、

BSP:binary space partitioning treeの一つです。

 

このkd-treeの特徴としては、

下記の表のように、Treeの作成には、平均でO(n)と、

データ量に対して線形比例の計算量が必要ですが、

一度作成したTreeに対して、最近傍点探索などを実施した場合、

計算量がO(logn)になることです。

操作 計算量の平均 計算量の最悪値
作成 O(n) O(n)
検索 O(logn) O(n)
挿入 O(logn) O(n)
削除 O(logn) O(n)

 

これにより、通常のBrute forceの探索に比べて、

データ量が増大しても、

あまり計算時間が増大することなく、

最近傍点を探索することが可能です。

 

加えて、最近傍点だけでなく、

ある点から、一定距離内の点の探索なども可能です。

 

k-d treeは任意の次元に対応できますが、

高次元になるほど線形探索と変わらなくなってしまうので、

探索対象の次元をD、データ数をnとすると、

n >> 2D となるのが望ましいそうです。

 

scipy.spatial.KDTreeの使い方

scipy.spatial.KDTreeの使い方の概要について説明します。

 

下記の例では、こちらのモジュールが

インポートされてていることを仮定しています。

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree

詳細は、公式ドキュメントを参照ください。

scipy.github.io

 

kdtreeの作成

下記の通り、二次元のtableデータを入力することで、

複数次元の点データに基づく、

kd-treeを作成することができます。

下記の例では、二次元の点列から、kd-treeを作っています。

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import cKDTree
points = np.random.random((20, 2))
kd_tree = KDTree(points)

 

n個のm次元のデータでkd-treeを作成する場合は、

n x mの二次元のデータを入力すればOKです。

 

一つ注意点としては、

入力するデータの数(n)に関しては、log n の探索が可能ですが、

mの次元数が大きくなるに従って、

計算の効率が悪くなることが知られています。

公式のドキュメントによると20次元以上のデータでも、

すでに性能の劣化が見られてしまうとのことです。

scipy.github.io

 

また、下記のように、作成したインスタンスのフィールドから、

データのサイズや、各次元のデータの最大値、最小値などが得られます。

print(f"{kd_tree.m=}")
print(f"{kd_tree.n=}")
print(f"{kd_tree.leafsize=}")
print(f"{kd_tree.size=}")
print(f"{kd_tree.maxes=}")
print(f"{kd_tree.mins=}")
# kd_tree.m=2
# kd_tree.n=100
# kd_tree.leafsize=16
# kd_tree.size=15
# kd_tree.maxes=array([0.99660085, 0.98942514])
# kd_tree.mins=array([0.01764816, 0.00777515])

最近傍点の探索

query関数を使います。

scipy.github.io

 

今回は、k=2を設定し、一番と二番目に近い点を探索します。

np.random.seed(20)
targets = np.random.random((3, 2))
points = np.random.random((100, 2))
kd_tree = KDTree(points)
dists, indexes = kd_tree.query(targets, k=2)
plt.plot(targets[:, 0], targets[:, 1], "xb")
plt.plot(points[:, 0], points[:, 1], "og")
for i in range(len(indexes)):
    for j in indexes[i]:
        plt.plot([points[j, 0], targets[i, 0]],
                 [points[j, 1], targets[i, 1]], "-r")
plt.show()

下記のような結果が得られます。

f:id:meison_amsl:20200614201615p:plain

 

ある点から一定範囲の点を探索

近い順番ではなく、ある一定の距離の範囲内の点を探索したい場合は、

query_ball_pointを使います。

scipy.github.io

 

今回はr=0.2以内の点を検索しました。

np.random.seed(20)
targets = np.random.random((3, 2))
points = np.random.random((100, 2))
kd_tree = KDTree(points)
indexes = kd_tree.query_ball_point(targets, 0.2)
plt.plot(targets[:, 0], targets[:, 1], "xb")
plt.plot(points[:, 0], points[:, 1], "og")
for i in range(len(indexes)):
    for j in indexes[i]:
        plt.plot([points[j, 0], targets[i, 0]],
                 [points[j, 1], targets[i, 1]], "-r")
plt.show()

f:id:meison_amsl:20200614202122p:plain

 

2つのkd-tree同士のある一定距離範囲内の点の探索

2つのkd-treeがあり、お互いの点がある一定範囲内の点を探索する場合は、

query_ball_treeを使います。

scipy.github.io

 

下記の例では、2つのkd-treeのお互いの点がr=0.2以下の点を探索しています。

np.random.seed(21701)
points1 = np.random.random((15, 2))
points2 = np.random.random((15, 2))
plt.figure(figsize=(6, 6))
plt.plot(points1[:, 0], points1[:, 1], "xk")
plt.plot(points2[:, 0], points2[:, 1], "og")
kd_tree1 = KDTree(points1)
kd_tree2 = KDTree(points2)
indexes = kd_tree1.query_ball_tree(kd_tree2, r=0.2)
for i in range(len(indexes)):
    for j in indexes[i]:
        plt.plot([points1[i, 0], points2[j, 0]],
                 [points1[i, 1], points2[j, 1]], "-r")
plt.show()

f:id:meison_amsl:20200614202358p:plain

 

1つのkd-tree内のある一定距離範囲内の点の探索

先程は2つのkd-treeの場合でしたが、

ある一つのkd-treeの中で、ある一定範囲内の点の探索をする場合は、

query_pairsを使います。

scipy.github.io

 

np.random.seed(21701)
points = np.random.random((20, 2))
plt.figure(figsize=(6, 6))
plt.plot(points[:, 0], points[:, 1], "xk")
kd_tree = KDTree(points)
pairs = kd_tree.query_pairs(r=0.2)
for (i, j) in pairs:
    plt.plot([points[i, 0], points[j, 0]],
             [points[i, 1], points[j, 1]], "-r")
plt.show()

f:id:meison_amsl:20200614203309p:plain

 

2つのkd-treeの各点の距離を計算する

2つのkd-treeのある距離以下の点の距離を計算したい場合は、

sparse_distance_matrixを使います。

scipy.github.io

 

np.random.seed(21701)
points1 = np.random.random((5, 2))
points2 = np.random.random((5, 2))
kd_tree1 = KDTree(points1)
kd_tree2 = KDTree(points2)
indexes = kd_tree1.query_ball_tree(kd_tree2, r=0.2)
sparse_distance_matrix = kd_tree1.sparse_distance_matrix(kd_tree2, 0.2,
                                                         output_type="ndarray")
print(sparse_distance_matrix)
# [(0, 1, 0.14538496) (0, 3, 0.10257199) (1, 0, 0.13491385)
# (1, 3, 0.18793787) (2, 0, 0.19262396) (3, 0, 0.14859639)
# (3, 1, 0.07076002) (3, 3, 0.04065851) (4, 0, 0.17308768)]

参考資料

github.com

atkg.hatenablog.com

github.com

github.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com


NumPy&SciPy数値計算実装ハンドブック (Pythonライブラリ定番セレクション)

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com