コンピュータサイエンス系勉強ノート

計算機科学に限らず日々学んだことを色々まとめていきます

Iterative Closest Point を試してみた

今回はIterative Closest Point(ICP)アルゴリズムを試してみました.ICPは点群Aと点群Bが与えられた際に点群Bを回転と平行移動させて点群Aに位置合わせするためのアルゴリズムだそうです。

具体的なアルゴリズムは以下の通り。

  1. 点群Bの各点について点群Aの中から最近傍の点を見つける。
  2. 点群Bの各点とAの各点が対応しているので、各対応点に移るような変換行列を計算する。
  3. 変換行列を使ってBを写像する。
  4. 指定回数分もしくは収束するまで以上の処理を繰り返す。

f:id:clientver2:20151105132537p:plain

ソースコード

各ステップでの回転行列と平行移動行列の計算は以下のコードで計算しました.コードの詳細は以下のサイトを参照のこと.
Finding optimal rotation and translation between corresponding 3D points | Nghia Ho

def calcRigidTranformation(MatA, MatB):
    A, B = copy(MatA), copy(MatB)

    centroid_A = mean(A, axis=0)
    centroid_B = mean(B, axis=0)

    A -= centroid_A
    B -= centroid_B

    H = dot(A.T, B)
    U, S, V = linalg.svd(H)
    R = dot(V.T, U.T)
    T = dot(-R, centroid_A) + centroid_B

    return R, T

ICPのコードはこちら

from scipy.spatial import KDTree
import numpy as np
from math import sin, cos

class ICP(object):
    def __init__(self, pointsA, pointsB):
        self.pointsA = pointsA
        self.pointsB = pointsB
        self.kdtree = KDTree(self.pointsA)

    def calculate(self, iter):
        old_points = np.copy(self.pointsB)
        new_points = np.copy(self.pointsB)

        for i in range(iter):
            neighbor_idx = self.kdtree.query(old_points)[1]
            targets = self.pointsA[neighbor_idx]
            R, T = calcRigidTranformation(old_points, targets)
            new_points = np.dot(R, old_points.T).T + T

            if  np.sum(np.abs(old_points - new_points)) < 0.000000001:
                break

            old_points = np.copy(new_points)

        return new_points

def icp_test():
    Y, X = np.mgrid[0:100:5, 0:100:5]
    Z = Y ** 2 + X ** 2
    A = np.vstack([Y.reshape(-1), X.reshape(-1), Z.reshape(-1)]).T

    R = np.array([
        [cos(50), -sin(50), 0],
        [sin(50), cos(50), 0],
        [0, 0, 1]
    ])

    T = np.array([5.0, 20.0, 10.0])
    B = np.dot(R, A.T).T + T

    icp = ICP(A, B)
    points = icp.calculate(3000)

    from matplotlib import pyplot
    from mpl_toolkits.mplot3d import Axes3D

    fig = pyplot.figure()
    ax = Axes3D(fig)

    ax.set_label("x - axis")
    ax.set_label("y - axis")
    ax.set_label("z - axis")

    ax.plot(A[:,1], A[:,0], A[:,2], "o", color="#cccccc", ms=4, mew=0.5)
    ax.plot(points[:,1], points[:,0], points[:,2], "o", color="#00cccc", ms=4, mew=0.5)
    ax.plot(B[:,0], B[:,1], B[:,2], "o", color="#ff0000", ms=4, mew=0.5)

    pyplot.show()

icp_test()

出力結果

f:id:clientver2:20151127160400p:plain
f:id:clientver2:20151127160401p:plain

灰色の点が元々の点群Aで,赤いのがそれを変換した点群Bです.黄緑色の点はBをAに位置合わせしたもので,ちゃんと重なっていることが分かります.