3次元の回転をクラスタリングする

背景

車の3次元姿勢を予測するKaggleのコンペをやる過程で、3次元の回転をクラスタリングすることで離散化し、回転を求める問題をクラス分類として解くアプローチを検討しました。結局は、車は自由に回転できるわけではなく、ほぼ道路に平行な回転しか自由度がないため、その回転角度をsin(θ), cos(θ)で回帰するモデルのほうが筋が良いためお蔵入りになりました。回転角のクラスタリングとかニーズ全くなさそうですが、やっていて面白かったのでメモ書きとして残しておきます。

ちなみに、クラス分類として解くのは、例えば回転に制限のない人工衛星の姿勢推定とかでは意味があったかもしれません。

クラスタリングアルゴリズム

回転を扱う場合、scipy.spatial.transform.Rotationを利用すると、オイラー角、クォータニオン、回転行列が全て簡単に扱えるので、これを使うのが良いと思います。

クラスタリングアルゴリズムとしては、明確にクラスタがあるわけではなく、回転空間を分割したいだけなので単純にkmeansを使います。

2つの回転の距離は、片方の回転からもう片方の回転に変化させる最小の角度で定義するのが自然ですが、この角度は、片方の逆変換でもう片方を回転させ、クォータニオンに変換してその角度として取得できます。

# code from https://www.kaggle.com/its7171/metrics-evaluation-script
def rot_dist(rot1, rot2):
    diff = Rotation.inv(rot2) * rot1
    w = np.clip(diff.as_quat()[-1], -1., 1.)
    w = (math.acos(w) * 360) / math.pi
    if w > 180:
        w = 360 - w
    return w

複数の回転角の平均は、文献¹で提案されています。内容はなるほど分からんなのですが、なんとこちらの記事に解説がありました！使うだけであれば、Matlab実装をPythonに移植した実装があるのでそれを使います。

# code from https://github.com/christophhagen/averaging-quaternions
# https://github.com/christophhagen/averaging-quaternions/blob/master/LICENSE
def average_rotations(rots):
    # Number of quaternions to average
    M = len(rots)
    Q = np.array([q.as_quat() for q in rots])
    A = np.zeros(shape=(4, 4))

    for i in range(M):
        q = Q[i,:]
        # multiply q with its transposed version q' and add A
        A = np.outer(q,q) + A

    # scale
    A = (1.0/M)*A
    # compute eigenvalues and -vectors
    eigenValues, eigenVectors = np.linalg.eig(A)
    # Sort by largest eigenvalue
    eigenVectors = eigenVectors[:,eigenValues.argsort()[::-1]]
    # return the real part of the largest eigenvector (has only real part)
    return Rotation.from_quat(np.real(eigenVectors[:,0]))

これらの距離と平均を利用してkmeanを行います。

def kmeans(samples, k, reduce, distance, max_iter=300):
    sample_num = len(samples)
    centroids  = [samples[i] for i in np.random.choice(sample_num, k)]
    
    for i in range(max_iter):
        dist = 0.0
        centroid_id_to_samples = defaultdict(list)

        for sample in samples:
            distances = [distance(sample, c) for c in centroids]
            nearest_id = np.argmin(np.array(distances))
            dist += distances[nearest_id]
            centroid_id_to_samples[nearest_id].append(sample)
            
        print(i, dist / sample_num)
            
        for k, v in centroid_id_to_samples.items():
            centroids[k] = reduce(v)
            
    return centroids

実際にこれを利用して、車の姿勢をクラスタリングしてみた結果が、KaggleのNotebookにありますので、参照してみて下さい。

下記のような形で同じ姿勢の車がクラスタリングされます（オクルージョンとか見切れがあって分かりにくいですが）。

1. F. Markley, Y. Cheng, J. Crassidis, and Y. Oshman, "Averaging Quaternions," in Journal of Guidance, Control, and Dynamics, vol. 30, no. 4, pp. 1193-1197, 2007. ↩