三次元回転の推定 #3D

金谷著「3次元回転―パラメータ計算とリー代数による最適化―」(https://www.kyoritsu-pub.co.jp/bookdetail/9784320113824) を読んで三次元回転の推定の実装を行いました．

概要

回転の推定ということで以下の内容を扱います．

第4章回転の推定I：等方性誤差
第5章回転の推定II：異方性誤差
第6章微分による最適化：リー代数の方法〜6.6

基本的に，直接式を書き下さずに式番号で示すので，本を参照しないとよくわからない内容かもしれませんが，その点はご了承ください（引用の範疇を超えると思っているので意図的に書いていません．実際面倒なのもありますが）．

（主要な）環境

Ubuntu 18.04
Python: 3.6.9
Jupyter Notebook: 1.0.0
NumPy: 1.17.4
SciPy: 1.3.3

リポジトリ

以下のコードは（構成の多少の変更を除いて）全て以下のリポジトリに上げています．

https://github.com/eduidl/3d-rotations

回転の誤差評価

本書7.1節に書かれている内容ですが，回転行列の真値 $R$ と推定値 $\hat R$ に対して， $|\hat R- R|$ を誤差として評価するのは不適切で， $\hat RR^{-1}$ を回転ベクトルに変換したときのその大きさで評価すべきという話です．
ということで，それをPythonのコードに起こすと以下の通りです．
なお，SciPyのscipy.spatial.transform.Rotationはクォータニオン・回転行列間の変換にも便利なので躊躇なく使っていきます．

util.py

from scipy import linalg
from scipy.spatial.transform import Rotation

def eval_R_error(estimated_R: np.ndarray, ideal_R: np.ndarray) -> np.float_:
    error_rot = Rotation.from_dcm(estimated_R @ ideal_R.T)
    return linalg.norm(error_rot.as_rotvec())

等方性誤差

notebook: https://github.com/eduidl/3d-rotations/blob/master/notebooks/isotropic_error.ipynb

まずは4章の内容で，等方性誤差を持つデータからの回転推定です．ここで等方性誤差というのは，

各データの誤差が独立で等方かつ一様な正規分布に従う
（¹ P.37より）

を意味しています．

データの準備

numpy.random.rand とか使って，適当にデータを作ってもいいんですが，結果のわかりやすさだったり，やった感が出るかだったりを気にして，みんな大好きスタンフォードうさぎを使いました．
データはhttp://graphics.stanford.edu/data/3Dscanrep/ からダウンロード．読み込みはOpen3Dを利用．

util.py

from pathlib import Path

import numpy as np
import open3d

def load_point_cloud(path: Path) -> np.ndarray:
    assert path.is_absolute() and path.exists()
    ply = open3d.io.read_point_cloud(str(path))
    return np.asarray(ply.points)

A = util.load_point_cloud(Path('../bunny/data/bun180.ply').resolve())
print(A.shape)
#=> (40251, 3)

itkwidgetsを使って可視化してみます．

うさぎですね．scipy.stats.special_ortho_groupを使って適当な回転行列を生成し，これを真値とします．

from scipy.stats import special_ortho_group

R = special_ortho_group.rvs(3)
print(R)
#=> [[-0.46875847  0.27761018  0.83856907]
# [ 0.31226178  0.94011334 -0.13667292]
# [-0.82629176  0.19778648 -0.52737314]]

先程手に入れたうさぎの点群データにRを用いて回転操作を施すとともに，等方的なノイズを加えます．

noise = np.random.normal(0, 3e-3, A.shape)
A_prime = A @ R.T + noise

元データと一緒に描画するとこんな感じ（赤が元データ）

データの準備が終わったので，早速AとA_primeから回転行列の推定を行います．

特異値分解による解法

4.3節の内容です．式(4.18), (4.19), (4.20)をコードにするだけで簡単です．

util.py

def estimate_R_using_SVD(A: np.ndarray, A_prime: np.ndarray) -> np.ndarray:
    assert A.shape[1] == A_prime.shape[1] == 3
    N = A.T @ A_prime  # 式 4.18
    U, s, Vh = linalg.svd(N)  # 式 4.19
    V = Vh.T
    return V @ np.diag([1, 1, linalg.det(V @ U)]) @ U.T  # 式 4.27

estimated_R = util.estimate_R_using_SVD(A, A_prime)
print(util.eval_R_error(estimated_R, R))
#=> 0.0003469883631373737

この推定値を使って点群同士のフィッティングをした結果が以下の通りです．良さそうですね．

四元数表示による解法

4.4節の内容です．まずは式(4.31)，やるだけ．

N = A.T @ A_prime
N_tilde = np.array([
    [
        N[0, 0] + N[1, 1] + N[2, 2],
        -N[2, 1] + N[1, 2],
        N[2, 0] - N[0, 2],
        -N[1, 0] + N[0, 1],
    ], [
        -N[2, 1] + N[1, 2],
        N[0, 0] - N[1, 1] - N[2, 2],
        N[1, 0] + N[0, 1],
        N[2, 0] + N[0, 2],
    ], [
        N[2, 0] - N[0, 2],
        N[1, 0] + N[0, 1],
        -N[0, 0] + N[1, 1] - N[2, 2],
        N[2, 1] + N[1, 2],
    ], [
        -N[1, 0] + N[0, 1],
        N[2, 0] + N[0, 2],
        N[2, 1] + N[1, 2],
        -N[0, 0] - N[1, 1] + N[2, 2],
    ],
])

あとは，$\tilde N$の最大固有値に対する固有ベクトルをクォータニオンとして回転行列に変換するだけです．
ちょっとした罠として，書籍のクォータニオンの順序と，SciPyのそれが異なります．これらの組合せに限らず，一般にクォータニオンの順序がどうなっているかには，注意した方がよいかもしれません．

w, v = linalg.eig(N_tilde)
max_vec = v[:, np.argmax(w)]
# 書籍では(w, x, y, z)だが，scipyでは(x, y, z, w)
r = Rotation.from_quat(max_vec[[1, 2, 3, 0]])
#=> 0.0003469883631370607

（プロットは大体同じなので割愛）

異方性誤差

notebook: https://github.com/eduidl/3d-rotations/blob/master/notebooks/anisotropic_error.ipynb

次に5章の内容で，異方性誤差を持つデータからの回転推定です．異方性誤差は等方性でない誤差で，簡単に言うと誤差の共分散行列が単位行列のスカラ倍にならないようなやつです．
なお共分散行列自体は既知としています．

データの準備

等方性誤差のところと同じなのでまず回転行列から．

ideal_R = special_ortho_group.rvs(3)
print(ideal_R)
# [[-0.9714901   0.08571768  0.22104179]
# [ 0.04373118  0.9811421  -0.18827575]
# [-0.23301197 -0.17324161 -0.95691837]]

今回は異方性誤差なので，先程とは違い，A, A_primeの両方にそれぞれの共分散行列に従ったノイズを加える必要があります．
ということで，今回はbunny/data/bun180.plyを真の値（本書でいう $\bar{\boldsymbol a}_\alpha$）とします．

A_bar = util.load_point_cloud(Path('../bunny/data/bun180.ply').resolve())

まずはA, A_primeが従うノイズの共分散行列を決める必要があります．今回は適当に，scipy.stats.random_correlationを使いました．

from scipy.stats import random_correlation

cov_a = random_correlation.rvs((0.5, 1.2, 1.3))
print(cov_a)
#=> [[ 1.         -0.25341891 -0.20405749]
# [-0.25341891  1.         -0.29006792]
# [-0.20405749 -0.29006792  1.        ]]

cov_a_prime = random_correlation.rvs((0.1, 0.2, 2.7))
print(cov_a_prime)
# [[ 1.          0.86487508 -0.88330608]
# [ 0.86487508  1.         -0.80110017]
# [-0.88330608 -0.80110017  1.        ]]

未知の値であるノイズレベルは適当に 3e-3 にしました（それ以上増やすとうさぎの耳がマージされたので，形状が大体保たれる範囲でできるだけ大きくした）．
共分散行列が手に入ったので，numpy.random.multivariate_normalを用いてノイズを生成し，擬似的な観測データを作成しました．

points_num = A_bar.shape[0]
noise_level = 3e-3
A = A_bar + noise_level * np.random.multivariate_normal(np.zeros(3), cov_a, points_num)
A_prime = A_bar @ ideal_R.T + noise_level * np.random.multivariate_normal(np.zeros(3), cov_a_prime, points_num)

ということで，A, A_primeを用いて回転推定を行っていきます．

特異値分解

5章には書かれていませんが，リファレンスとしてやっておきます．

R1 = util.estimate_R_using_SVD(A, A_prime)
print('error:', util.eval_R_error(R1, ideal_R))
#=> error: 0.00021641128777628564

普通に推定できているという（共分散行列をもっと極端にしないといけないのだろうか）．

FNS法による最適化

5.3節と5.4節の内容です．

式 5.22

$\boldsymbol\xi^{(1)},\boldsymbol\xi^{(2)},\boldsymbol\xi^{(3)}$ をまとめて，3×（点の数）×4のテンソルにしています．

Xi = np.stack([
    np.hstack([
        A_prime[:, [0]] - A[:, [0]],
        np.zeros([points_num, 1]),
        -(A_prime[:, [2]] + A[:, [2]]),
        A_prime[:, [1]] + A[:, [1]]
    ]),
    np.hstack([
        A_prime[:, [1]] - A[:, [1]],
        A_prime[:, [2]] + A[:, [2]],
        np.zeros([points_num, 1]),
        -(A_prime[:, [0]] + A[:, [0]])
    ]),
    np.hstack([
        A_prime[:, [2]] - A[:, [2]],
        -(A_prime[:, [1]] + A[:, [1]]),
        A_prime[:, [0]] + A[:, [0]],
        np.zeros([points_num, 1])
    ])
])
print(Xi.shape)
#=> (3, 40251, 4)

式 5.28

誤植があるので注意．一部正誤表に載っていなかった気がする，と思って見に行ったら追加されていた．
正誤表P.5の

p.57，式 (5.28)
p.82–83，以下の4式について $V[a_\alpha]$ を $V_0[a_\alpha]$に置き換え

は自分が報告したやつです（謎の自慢）．

閑話休題．$\boldsymbol T^{(1)},\boldsymbol T^{(2)},\boldsymbol T^{(3)}$ を1つのテンソルにまとめました．

T = np.array([
    [
        [-1,  0,  0,  1,  0,  0],
        [ 0,  0,  0,  0,  0,  0],
        [ 0,  0, -1,  0,  0, -1],
        [ 0,  1,  0,  0,  1,  0]
    ], [
        [ 0, -1,  0,  0,  1,  0],
        [ 0,  0,  1,  0,  0,  1],
        [ 0,  0,  0,  0,  0,  0],
        [-1,  0,  0, -1,  0,  0]
    ], [
        [ 0,  0, -1,  0,  0,  1],
        [ 0, -1,  0,  0, -1,  0],
        [ 1,  0,  0,  1,  0,  0],
        [ 0,  0,  0,  0,  0,  0]
    ]
])
print(T.shape)
#=> (3, 4, 6)

式 5.32

$V_0^{(kl)}[\boldsymbol \xi_\alpha]\ (k,l=1,2,3)$ を1つのテンソルにまとめました．

from itertools import product

cov_joined = linalg.block_diag(cov_a, cov_a_prime)

V_0 = np.zeros([3, 3, T.shape[1], T.shape[1]])
for k, l in product(range(3), repeat=2):
    V_0[k, l] = T[k] @ cov_joined @ T[l].T
print(V_0.shape)
#=> (3, 3, 4, 4)

式 5.40

この辺りから数式の，$\sum_\alpha^N$ をforを使わずに書くのに少し苦労したりしました．
引数のWは $W_\alpha^{(kl)}$，qは$\boldsymbol q$です．

def calc_M(W, Xi):
    dim = Xi.shape[2]
    M = np.zeros([dim, dim])
    for k, l in product(range(3), repeat=2):
        M += W[k, l] * Xi[k].T @ Xi[l]
    return M

def calc_L(W, q, Xi, V_0):
    _, points_num, dim = Xi.shape
    # 下3行は式 5.38
    V = np.zeros([3, points_num])
    for k, l in product(range(3), repeat=2):
        V[k] += W[k, l] * Xi[l] @ q
    L = np.zeros([dim, dim])
    for k, l in product(range(3), repeat=2):
        L += np.inner(V[k], V[l]) * V_0[k, l]
    return L

メイン関数

必要な関数が出揃ったのでメインの実装をします．コメントの# stepは本書の説明のステップ数に対応しています．まあ大して難しくないはずです．

def FNS_method(Xi, V_0):
    # step 1
    q0 = np.zeros(4)
    W = np.eye(3)
    iters = 1

    while True:
        # step 2
        X = calc_M(W, Xi) - calc_L(W, q0, Xi, V_0)  # 式 5.41
        # step 3
        w, eigenvecs = linalg.eigh(X)
        q = eigenvecs[:, np.argmin(w)]
        # step 4
        if np.allclose(q, q0) or np.allclose(q, -q0):
            return q, iters
        W_inv = np.zeros_like(W)
        for k, l in product(range(3), repeat=2):
            W_inv[k, l] = np.inner(q, V_0[k, l] @ q)
        W = linalg.inv(W_inv)
        q0 = q
        iters += 1

q, iters = FNS_method(Xi, V_0)
R2 = Rotation.from_quat(q[[1, 2, 3, 0]]).as_dcm()
print('iterations:', iters)
print('error:', util.eval_R_error(R2, ideal_R))
# iterations: 4
# error: 0.0001321067088523419

特異値分解は約 $0.00021$ だったので少し良くなっていますね．

同次拘束条件による解法

5.5節の内容です．手法としては拡張FNS法ということになります．

式 5.46

$\boldsymbol\xi^{(1)},\boldsymbol\xi^{(2)},\boldsymbol\xi^{(3)}$ を1つのテンソルにまとめています．

zeros = np.zeros([points_num, 3])
Xi = np.stack([
    np.hstack([A, zeros, zeros, -A_prime[:, [0]]]),
    np.hstack([zeros, A, zeros, -A_prime[:, [1]]]),
    np.hstack([zeros, zeros, A, -A_prime[:, [2]]])
])
del zeros
print(Xi.shape)
#=> (3, 40251, 10)

式 5.55

$\boldsymbol T^{(1)},\boldsymbol T^{(2)},\boldsymbol T^{(3)}$ を1つのテンソルにまとめました．

T = np.zeros([3 ,10, 6])
for i in range(3):
    T[i, i * 3, 0] = T[i, i * 3 + 1, 1] = T[i, i * 3 + 2, 2] = 1
    T[i, 9, 3 +  i] = -1
print(T.shape)
#=> (3, 10, 6)

式 5.58

$V_0^{(kl)}[\boldsymbol \xi_\alpha]\ (k,l=1,2,3)$ を1つのテンソルにまとめました．

V_0 = np.zeros([3, 3, T.shape[1], T.shape[1]])
for k, l in product(range(3), repeat=2):
    V_0[k, l] = T[k] @ cov_joined @ T[l].T
print(V_0.shape)
#=> (3, 3, 10, 10)

4次元部分空間へ射影する関数

本書だと，

FNS法の各反復ステップで，$\boldsymbol\theta$の10次元空間において，解$\boldsymbol\theta$を$\nabla_\boldsymbol\theta\phi_i,\ i=1,\dots,6$に直行する4次元部分空間に射影する．
（¹ P.65より）

と書かれているところです．行間が読めず実装できなかったので，Extended FNS for Constrained Parameter Estimation を少し読みました．

orthogonal_basisは本書の式 5.50を偏微分した結果です．Gram–Schmidtの正規直交化をscipy.linalg.qrをしてなんやかんやします．

def projection_matrix(u):
    orthogonal_basis = np.array([
        [u[1], u[0], 0, u[4], u[3], 0, u[7], u[6], 0, 0],
        [0, u[2], u[1], 0, u[5], u[4], 0, u[8], u[7], 0],
        [u[2], 0, u[0], u[5], 0, u[3], u[8], 0, u[6], 0],
        [2*u[0], 0, 0, 2*u[3], 0, 0, 2*u[6], 0, 0, -2*u[9]],
        [0, 2*u[1], 0, 0, 2*u[4], 0, 0, 2*u[7], 0, -2*u[9]],
        [0, 0, 2*u[2], 0, 0, 2*u[5], 0, 0, 2*u[8], -2*u[9]],
    ]).T

    constraint_num = orthogonal_basis.shape[1]
    # Gram–Schmidt process
    Q, _ = linalg.qr(orthogonal_basis)
    P = np.eye(10)
    for i in range(6):
        P -= np.outer(Q[:, i], Q[:, i])
    return P, constraint_num

メイン関数

必要な関数が出揃ったのでメインの実装をします．コメントの# stepは論文のProcedureの項のステップ数に対応しています．流れとしてはFNS法と大体同じだと感じられるのではないでしょうか．

def EFNS_method(Xi, V_0):
    # step 1
    u = np.array([1., 0., 0.,
                  0., 1., 0.,
                  0., 0., 1., 1.])
    u /= linalg.norm(u)
    W = np.eye(3)
    iters = 1

    while True:
        # step 2
        M = calc_M(W, Xi)
        L = calc_L(W, u, Xi, V_0)
        # step 3, 4
        P, constraint_num = projection_matrix(u)
        # step 5
        X = P @ (M - L) @ P
        # step 6
        w, vecs = linalg.eigh(X)
        vecs = vecs[:, np.argsort(w)[:constraint_num + 1]]
        # step 7
        u_hat = np.zeros_like(u)
        for i in range(constraint_num + 1):
            u_hat += np.inner(u, vecs[:, i]) * vecs[:, i]
        # step 8
        u_prime = P @ u_hat
        u_prime /= linalg.norm(u_prime)
        if np.allclose(u_prime, u) or np.allclose(u_prime, -u):
            return u_prime, iters
        u += u_prime
        u /= linalg.norm(u)
        W_inv = np.zeros_like(W)
        for k, l in product(range(3), repeat=2):
            W_inv[k, l] = np.inner(u, V_0[k, l] @ u)
        W = linalg.inv(W_inv)
        iters += 1

u, iters = EFNS_method(Xi, V_0)
R3 = u[:-1].reshape(3, 3) / u[-1]
print('iterations:', iters)
print('error:', util.eval_R_error(R3, ideal_R))
# iterations: 24
# error: 0.00013210670343442174

iterationsはFNS法に比べて大分大きくなっていますが，射影処理を挟んでいるためでしょうか？

リー代数の方法

章は変わり，6.6節の内容です．

式 5.11

式 5.12と併せて，章を跨いでいて見つけるのに時間がかかった（前章の実装で，これらの式は直接使っていないし）．

def calc_J(A, A_prime, cov_a, cov_a_prime, R):
    W = calc_W(cov_a, cov_a_prime, R)
    E = A_prime - A @ R.T
    return (E * (E @ W.T)).sum()

式 5.12

def calc_W(cov_a, cov_a_prime, R):
    return linalg.inv(R @ cov_a @ R.T + cov_a_prime)

式 6.42

numpy.crossの使い方難しい．ここは，全て第1引数と第2引数のshapeが同じだからまだましですが．

def calc_g(A, A_prime, R, W, cov_a):
    ART = A @ R.T
    EWT = (A_prime - ART) @ W.T
    g = (-np.cross(ART, EWT, axis=1) + np.cross(EWT, EWT @ (R @ cov_a @ R.T), axis=1)).sum(axis=0)
    return g

式 6.46

1週間後に書けって言われたらまた悩みそうなぐらい，よくわかっていない．
ちなみに，ART.shape == (40251, 3), W.shape == (3, 3), tmp.shape == (40251, 3, 3) です．
より数式通りに書いたのをコメントに書いています．

def calc_H(A, R, W):
    ART = A @ R.T
    tmp = np.stack([
        # np.cross(ART, W[:, 0], axisa=1, axisb=0, axisc=1)
        np.cross(ART, W[[0]], axis=1),
        np.cross(ART, W[[1]], axis=1),
        np.cross(ART, W[[2]], axis=1),
    ], axis=2)
    # np.cross(tmp, ART.reshape(*ART.shape, 1), axisa=2, axisb=1, axisc=2).sum(axis=0)
    return np.cross(tmp, ART.reshape(-1, 1, 3), axis=2).sum(axis=0)

これよくわからない．

# OK
np.cross(ART, W[:, 0], axisa=1, axisb=0, axisc=1)
# NG
np.cross(tmp, ART, axisa=2, axisb=1, axisc=2)

回転の指数関数表示

6.3節にあるような式を直接使うことはなく大抵ロドリゲスの公式を使うということなので，そんな感じで実装します．

def skew_matrix(v: np.ndarray) -> np.ndarray:
    return np.array([
        [0, -v[2], v[1]],
        [v[2], 0, -v[0]],
        [-v[1], v[0], 0]
    ])

def exponential_map(omega: np.ndarray) -> np.ndarray:
    assert omega.shape == (3,)
    t = linalg.norm(omega)
    A_omega = skew_matrix(omega / t)
    return np.eye(3) + math.sin(t) * A_omega + (1 - math.cos(t)) * A_omega @ A_omega

メイン関数

iterationsの定義が自明でなかったのでそこはあきらめた．

def lie_optimize(A, A_prime, cov_a, cov_a_prime):
    # step 1
    R = init_R = util.estimate_R_using_SVD(A, A_prime)
    J = init_J = calc_J(A, A_prime, cov_a, cov_a_prime, R)
    c = 0.0001

    while True:
        W = calc_W(cov_a, cov_a_prime, R)
        # step 2
        g = calc_g(A, A_prime, R, W, cov_a)
        H = calc_H(A, R, W)
        while True:
            # step 3
            omega = linalg.solve(H + c * np.eye(3), -g)
            # step 4
            new_R = util.exponential_map(omega) @ R
            # step 5
            new_J = calc_J(A, A_prime, cov_a, cov_a_prime, new_R)
            if new_J <= J:
                break
            c *= 10
        # step 6
        if linalg.norm(omega) < 1e-10:
            return new_R, new_J, init_R, init_J
        R = new_R
        J = new_J
        c /= 10

R4, J, init_R, init_J = lie_optimize(A, A_prime, cov_a, cov_a_prime)
print('initial error:', util.eval_R_error(R1, ideal_R))
print('final error:', util.eval_R_error(R4, ideal_R))
print('J:', init_J, '->', J)
# initial error: 0.00021641128777628564
# final error: 0.00013210660251496964
# J: 1.0875904572171673 -> 1.087588703231312

金谷健一「3次元回転―パラメータ計算とリー代数による最適化―」 ↩