3次元回転の最適化計算 - デジタルリアクタ合同会社

Contents

はじめに
リー代数とは
Pythonで実験してみる
まとめ

はじめに

この記事では、あるコスト関数を最小化するための3次元回転の最適化について、リー代数の考え方を用いて理論的な背景を解説し、Pythonで実装を行います。詳細については、書籍「3次元回転(金谷健一著)」が参考になります。

この記事を書いたひと

デジタルリアクタ合同会社代表
望月優輝 / Yuki Mochizuki

機械学習・統計、数値計算などの領域を軸としたソフトウェアエンジニアリングを専門としています。スタートアップからグローバル企業まで、さまざまなスケールの企業にて、事業価値に直結する計算システムを設計・構築してきました。主に機械学習の応用分野において、出版・発表、特許取得等の実績があり、また、IT戦略やデータベース技術等の情報処理に関する専門領域の国家資格を複数保有しています。九州大学理学部卒業。日本ITストラテジスト協会正会員。

対象読者:

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

記事のポイント:

リー代数の概念を、具体例を交えながら解説
Pythonコードによる具体的な実装例を提供
特異値分解(SVD)を用いた回転行列の補正について解説
最急降下法による最適化の過程を可視化

リー代数とは

リー代数とは、無限小回転が生成する線形空間のことです。無限小回転の合成は、有限回転とは異なり、合成の順序が結果に影響を与えません（可換性）。

有限回転に無限小回転を追加で合成することは、その有限回転付近での無限小回転による線形な変化を捉えることに相当します。無限小回転であるため、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 は厳密な回転行列ではないため、最終的に得られる結果も回転行列からわずかにずれる可能性があります。そこで、得られた結果を最も近い回転行列に補正する必要があります。

Pythonで実験してみる

ここでは、Pythonを用いてリー代数に関する様々な実験を行います。

無限小回転の行列

まず、無限小回転が本当に 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]])

この結果から、A が反対称行列（対角成分が0で、対角線を挟んで符号が反転する行列）に近いことが確認できます。回転軸を変更しても同様の結果が得られます。

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

通常、フロベニウスノルムを最小化する回転行列が選択されます。これは、特異値分解（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 も単位行列に近づきました。

最適化

ここでは、最急降下法を用いてコスト関数を最小化します。
最適化の際には、常に最新の回転の近傍で微分を計算することに注意してください。微分の計算には自動微分ライブラリを使用しています。

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]])

最適化の結果、正解の回転行列に近い値が得られていることがわかります。

まとめ

この記事では、リー代数を用いた3次元回転の最適化について解説し、Pythonでの実装例を示しました。リー代数の考え方と実装方法を理解することで、様々な最適化問題に応用できる可能性があります。また、特異値分解(SVD)を用いることで、計算結果を回転行列に補正できることを学びました。

お気軽にご相談ください！

弊社のデジタル技術を活用し、貴社のDXをサポートします。

基本的な設計や実装支援はもちろん、機械学習・データ分析・3D計算などの高度な技術が求められる場面でも、最適なソリューションをご提案します。

初回相談は無料で受け付けております。困っていること、是非お聞かせください。

サービスの詳細はこちら

お問い合わせフォームへ