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.cKDTreeというモジュールの使い方を紹介したいと思います。

scipy.github.io

 

ちなみに、scipy.spatial.KDTreeというモジュールがありますが、

scipy.github.io

こちらは、純Pythonで実装されたkdtreeで、

cKDTreeがcythonで高速化されたバージョンのKDTreeです。

こちらのroadmapにある通り、将来的にはcKDTreeがKDTreeに

置き換えられる予定ですので、cKDTreeを紹介します。

scipy.github.io

 

現時点では、この移行のために、

KDTreeもcKDTreeもほぼ同じAPIをもっていますので、

現時点では、計算が高速なcKDTreeを使っておくのが良いと思います。

(また最近追加された機能はcKDTreeにしか追加されないようになっているため、

若干、cKDTreeのほうが多機能です。)

 

kdtreeとは

kdtreeは、(k dimensional tree)

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の探索に比べて、

データ量が増大しても、

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

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

 

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

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

 

scipy.spatial.cKDTreeの使い方

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

 

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

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

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

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

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 = cKDTree(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 = cKDTree(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 = cKDTree(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 = cKDTree(points1)
kd_tree2 = cKDTree(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 = cKDTree(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 = cKDTree(points1)
kd_tree2 = cKDTree(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

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