3次元回転の最適化計算

はじめに

点群の位置合わせやカメラ姿勢の推定では、コスト関数を最小化する回転行列を探す場面が出てきます。厄介なのは、回転行列の9つの成分を自由に動かせないことです。直交性の制約があるため、成分を勾配方向に少し動かすだけで回転行列ではなくなってしまいます。この制約を自然に扱う道具がリー代数で、回転の更新を3つのパラメータの探索に置き換えられます。ここではその考え方を整理したうえで、最急降下法による最適化をPythonで実装し、正解の回転に収束する様子まで確かめます。理論の詳細は書籍「3次元回転」（金谷健一著）に詳しく書かれています。

対象読者:

• 3次元回転の基礎を理解している方
• 最適化アルゴリズムに興味がある方
• Pythonでの数値計算に慣れている方

記事のポイント:

• リー代数（無限小回転）の考え方を具体例で整理
• 特異値分解（SVD）による回転行列への補正
• 最急降下法による最適化のPython実装と収束の確認

リー代数とは

リー代数とは、無限小回転が生成する線形空間のことです。有限回転と違い、無限小回転どうしの合成は順序を入れ替えても結果が変わりません（可換性）。

有限回転に無限小回転をさらに合成する操作は、その有限回転の近くでの線形な変化を取り出すことにあたります。無限小回転なので、2次以上の変化は無視されます。この無限小回転の全体が、その有限回転での接空間（接ベクトル空間）です。

無限小回転は、ある反対称行列 A に十分小さい dt を乗じた I + Adt で表現されます。反対称行列は3つのパラメータで記述されるため、これらのパラメータを変化させて、有限回転の近傍でコスト関数を最も小さくする方向を探索し、有限回転を更新します。この更新を繰り返せば、コスト関数を最小化する回転が求まります。

ここで、反対称行列Aは以下のように表されます。

$$A = \begin{bmatrix} 0 & -w_3 & w_2 \\ w_3 & 0 & -w_1 \\ -w_2 & w_1 & 0 \end{bmatrix}$$

ここで、w1, w2, w3 は回転のパラメータです。

ただし、I + Adt は厳密な回転行列ではないため、最終的に得られる結果も回転行列からわずかにずれることがあります。そこで、得られた結果は最も近い回転行列に補正して使います。

無限小回転のずれが反対称行列になることを数値で見る

まず、無限小回転が本当に I + Adt で表現されるのかを、非常に小さい角度 1e-6 で検証します。

from pyquaternion import Quaternion
import numpy as np

q = Quaternion(axis=np.array([1.0, 1.0, 2.0]), angle=1e-6)
q.rotation_matrix - np.eye(3)

実行結果は次の通りです。

# 出力
array([[-4.16666701e-13, -8.16496498e-07,  4.08248457e-07],
       [ 8.16496664e-07, -4.16666701e-13, -4.08248124e-07],
       [-4.08248124e-07,  4.08248457e-07, -1.66644476e-13]])

対角成分がほぼ0で、対角線を挟んで符号が反転しており、A が反対称行列に近いことが分かります。回転軸を変えても同様の結果になります。

最も近い回転行列への補正

最適化に入る前に、回転行列からずれた計算結果を引き戻す手続きを用意しておきます。補正先には、元の行列との差のフロベニウスノルムが最小になる回転行列を選びます。これは、特異値分解（SVD）を行った際に、特異値をすべて1に置き換える操作と等価です。

回転行列 R は逆行列が転置行列に等しいので、回転行列かどうかは R @ R^T が単位行列になるかで確かめられます。ここで、@ はNumPyの行列積です。

まず、回転行列にノイズを加えて崩してみます。

rot = Quaternion(axis=np.array([1.0, 1.0, 2.0]), angle=np.pi/3).rotation_matrix
rot_with_noise = rot + np.random.rand(3, 3) * 0.1

print(rot)
print(rot_with_noise)

print(rot@rot.T)
print(rot_with_noise@rot_with_noise.T)

出力は次のようになります。

# 出力

# 回転行列
[[ 0.58333333 -0.62377345  0.52022006]
 [ 0.79044011  0.58333333 -0.18688672]
 [-0.18688672  0.52022006  0.83333333]]
# 崩した回転行列
[[ 0.60881564 -0.58417763  0.56339398]
 [ 0.83009892  0.61178191 -0.16292351]
 [-0.15160353  0.54837406  0.88222256]]
# 回転行列のR@R^T
[[1.00000000e+00 1.78147168e-17 4.00006039e-17]
 [1.78147168e-17 1.00000000e+00 4.71671236e-18]
 [4.00006039e-17 4.71671236e-18 1.00000000e+00]]
# 崩した回転行列のR@R^T
[[1.02933276 0.05619777 0.08439241]
 [0.05619777 1.08988539 0.0659046 ]
 [0.08439241 0.0659046  1.10201438]]

rot_with_noise では、ノイズによって R @ R^T が単位行列からずれています。

次に、この行列を補正します。np.linalg.svd で特異値分解を行うと、回転行列に近い行列では特異値がすべて1に近く、回転行列から離れるにつれて1から離れていきます。特異値をすべて1に置き換えて行列を再構成すると、U と Vt は直交行列なので、得られる行列は回転行列になります。

U, S, Vt = np.linalg.svd(rot_with_noise)
rot_fixed = U@Vt
print(rot_fixed)
print(rot_fixed@rot_fixed.T)

# 出力

# 補正後回転行列
[[ 0.58765643 -0.61308353  0.52800427]
 [ 0.78635463  0.58643711 -0.19426247]
 [-0.19054218  0.5293582   0.82672461]]
# 補正後回転行列のR@R^T
[[ 1.00000000e+00  3.10964377e-16 -4.31956114e-16]
 [ 3.10964377e-16  1.00000000e+00 -5.94847355e-17]
 [-4.31956114e-16 -5.94847355e-17  1.00000000e+00]]

補正後の回転行列は元の回転行列に近い値となり、R @ R^T も単位行列に戻りました。

最急降下法で回転を推定する

準備が整ったので、最急降下法でコスト関数を最小化します。ランダムな20点を既知の回転で回した点群を用意し、その回転を推定する問題を解きます。各反復では、常に最新の回転の近傍で微分を計算し直します。微分の計算には自動微分ライブラリを使っています。

import autograd.numpy as np
from autograd import grad, jacobian

def fix_rot(rot):
    U, S, Vt = np.linalg.svd(rot)
    rot_fixed = U@Vt
    return rot_fixed

def make_A_from_w(w):
    w1, w2, w3 = w
    A = np.array([
        [  0, -w3, w2],
        [ w3,   0, -w1],
        [-w2, -w1,  0]
    ])
    return A

def cost(xyz, xyz_rotated, rot):
    return np.sum((xyz_rotated - rot@xyz)**2)

n = 20
xyz = np.random.rand(3, n)
rot_gt = Quaternion(axis=np.array([1.0, 1.0, 2.0]), angle=np.pi/3).rotation_matrix
xyz_rotated = rot_gt @ xyz
rot = np.eye(3)

alpha = 0.005
for t in range(1000):
    if t%10 == 0:
        print("cost:", cost(xyz, xyz_rotated, rot))

    def f(w):
        A = make_A_from_w(w)
        rot_ = (A + np.eye(3)) @ rot
        return cost(xyz, xyz_rotated, rot_)

    jac = jacobian(f)

    w0 = np.array([0., 0., 0.])
    j = jac(w0)

    A = make_A_from_w(-j * alpha)
    rot = (A + np.eye(3)) @ rot

    rot = fix_rot(rot)

実行すると、コストが単調に減少していきます。

# 出力
cost: 3.9702549344251628
cost: 1.6704174593991687
cost: 1.0027392982419492
cost: 0.6002849027408019
cost: 0.35364862052704077
cost: 0.20625092594736405
cost: 0.11962215280830635
cost: 0.06921272409593454
cost: 0.04004941308338197
cost: 0.023234351825969623
:
:
cost: 0.0004929792016906296
cost: 0.0004929792016906142
cost: 0.0004929792016906262
cost: 0.0004929792016905991

正解の回転行列は以下の通りです。

array([[ 0.58333333, -0.62377345,  0.52022006],
       [ 0.79044011,  0.58333333, -0.18688672],
       [-0.18688672,  0.52022006,  0.83333333]])

一方、最適化で得られた回転行列は次の値になりました。

array([[ 0.58931866, -0.62918377,  0.52054695],
       [ 0.79657117,  0.58803591, -0.19202649],
       [-0.18970697,  0.52627442,  0.83512569]])

各成分の差は最大でも0.01未満で、正解の回転行列にほぼ収束しています。

まとめ

回転行列の直交性という扱いにくい制約は、リー代数を使えば3つのパラメータの無制約な探索に置き換えられます。今回の実験では、この置き換えと特異値分解による補正だけで、単純な最急降下法でも正解の回転にほぼ一致する解が得られました。点群の位置合わせやカメラ姿勢の推定など、回転を変数に含む最適化問題にはこの枠組みがそのまま使えます。実務の規模で解くなら、収束を速めるためにガウス・ニュートン法やレーベンバーグ・マーカート法へ進むのが次の一手です。