しゃけのブログ

某大学院で研究している人の備忘録

状態推定のためのLie群上の最適化1

多様体最適化

LSD-SLAMやDSOなど，SLAM系の論文を読んでいると必ず出てくるLie群・Lie代数．．．

自己位置推定における推定対象となる3次元空間上の姿勢は，単なる並進と回転を合わせた単なる6次元実数空間に一様に広がっているわけではない．

例えば，3次元回転行列がなす空間 \mathrm{SO}(3)は，9次元ベクトルに次のような制約を持っている．

\begin{array} \ \mathrm{SO}(3) = \{\mathbf{R} \in \mathbb{R}^{3 \times 3}| \mathbf{R}^\top \mathbf{R} = \mathbf{R} \mathbf{R}^\top = \mathbf{E}_3, \mathbf{R}^{-1} = \mathbf{R}^\top\} \end{array}

このような3次元回転の最適化では，例えばNewton法などの勾配降下アルゴリズムで9次元ベクトルとして最適化できない． なぜなら，パラメータ更新後のベクトルが回転行列とは限らなくなってしまうからである．

別の最適化の方法として，各軸の回転角 \theta_i \in [-\pi, \pi]を3次元ベクトルとして，次のような空間の上での最適化を行う．

\begin{array} \ \{R_z (\theta_z) R_y(\theta_y) R_x(\theta_x) | (\theta_x, \theta_y, \theta_z) \in [-\pi, \pi]^3\} \end{array}

ここで， R_{\{x, y, z\}}は各軸に対する回転を表す．

しかし，この空間上の最適化にも問題がある．異なる3次元ベクトルでも同じ回転を表してしまうのである． また，ジンバルロック（ジンバル - Wikipedia）という問題もある．


ここで，Lie群，Lie代数という概念が使われる． この概念を使った最適化では，3次元実数空間 \mathbb{R}^3と \mathrm{SO}(3)との間の全単射（しかも逆写像も存在）を用意することによって， \mathrm{SO}(3)の最適化を3次元空間上の最適化と同等に行う．

最終的な目的としては，Gauss-Newton法をLie代数上で行う方法について理解したい．

Lie theoryは複雑で広範だけど，状態推定に必要な部分に絞って説明している"A micro Lie theory for state estimation in robotics"という解説論文が大変わかりやすかったので，その備忘録． arxiv.org

この章の内容

まず，姿勢変数がなす空間として，「Lie群」を導入する．

そして，3次元空間をなす空間として，Lie群に付随する「Lie代数」について定義する．

さらに，Lie群とLie代数の間の写像として，「指数写像」を導入する．

Intro

ロボットの姿勢を推定する問題（自己位置推定やSLAMなど）では，回転行列や剛体変換行列を推定対象とすることが多いと思う

これらの行列の最適化を考えたとき，これらの行列を展開して，9次元ベクトルや16次元ベクトルとして最適化するとうまくいかない．

これらは例えば次のような制約を持っている．

また，「合成に順序がある」などの性質も持っている．とにかく単純な最適化じゃうまくいかない．

その行列のなす空間は直観的にはぐにゃぐにゃしている，とイメージしてもらえればよい． よくある，回転行列などをパラメータにした適当なloss関数の最適化を考えたとき，ぐにゃぐにゃ空間上でパラメータ更新をすることは難しいというわけだ．

例えば，下図のような空間を回転行列がなすとする． 9次元ベクトル空間でこれが描画されているとして，空間全体のうちで一部しか回転行列に属さない．

その一部分上でのみ動かせる最適化を考えることが難しいということだ．


姿勢を推定する問題，例えば回転行列を求める問題の解き方は，直観的に次のようなものがあるように考えられる．

  • Closed-form solution: 行列を構成するパラメータを独立に扱った連立方程式を解く．パラメータに対して制約がない（解が回転行列となるとは限らない）．

  • Constrained non-linear optimization: 制約項を付けた最適化アルゴリズムを使う．初期値依存，局所解の存在．


そこで，ぐにゃぐにゃ空間（Lie群）を，最適化に適したまっすぐな空間（Lie代数）に引き伸ばしてして最適化することを考える． このまっすぐな空間は，元の空間を引き延ばしただけなので，等価な空間として考えられる． （Tayler展開のようにまっすぐな平面で近似するようなアイデアではなく，要素同士が一対一対応するように3次元空間と対象の空間との同相写像を導入する）．

この空間は，元の空間の自由度（3D回転行列が対象なら3）の軸を持ったまっすぐなユークリッド空間と考えることができる． この空間では，パラメータを自由に動かすことができるため，元のLie群上の最適化が簡単な問題へと帰着される．

f:id:shuckle-rbtech:20210524160047p:plain
イメージ


Lie群の定義

回転行列や剛体変換行列とはそもそもなんなのか．

これらの空間を考える上で，その性質を性質を一言でまとめて抽象的に扱いたい．

例として，回転行列を考える． 回転行列には次のような性質がある．

  • 回転行列を回転する（回転行列を左からかける）と回転行列になる

  • 単位行列は回転しない回転行列にあたる

  • 任意の回転に対して，元に戻す回転行列が存在する．

これは，群の性質そのものである．

では，回転行列や剛体変換行列が群をなしていたと．

これに加えて，実は空間が滑らかであるという性質もこれらの行列にはある．

この，滑らかな群はLie群と呼ばれている．

Lie群の定義

A Lie group \mathcal{G} is a smooth manifold whose elements satisfy the group axioms

(訳）群の公理を満たす滑らかな多様体

「多様体」とは，「任意の点について，それを含む座標近傍が定義できるハウスドルフ空間」のことであるが，話が複雑になりがちなので，「あらゆる場所に局所座標系（ユークリッド座標系）が描ける空間」と考えればいいと思う．

群の公理は次の通り．

Lie群 (\mathcal{G}, \circ) （ \mathcal{G}は集合で \circ は演算）は次の性質を満たしている． ただし， \mathcal{X}, \mathcal{Y}, \mathcal{Z} \in \mathcal{G}

一方で，滑らかな多様体とかいう一見あいまいな単語が出てきた．

「滑らかさ」の定義

The smoothness of the manifold implies the existence of unique tangent space at each point

(訳) 各点で唯一の接空間が存在する．

接空間とは，直観的にはその点に対する接平面のこと． 実際は多様体は3次元空間ではないので，接平面ではないのだが，概念的には似ているのでそういうイメージを持っていたい．

接空間については，次章で定義を行う．

つまり，各変数の偏微分を並べたものが存在しているということなので，「滑らか」という表現につながる．

今後，ここで定義したLie群を回転行列や剛体変換行列の抽象化として，その上での最適化を考えていく．

Lie代数

さて，Lie群が回転行列や剛体変換行列の抽象化であるという話を導入で話したが，ぐにゃぐにゃしているLie群をまっすぐに引き延ばして，最適化に適する空間へと変換することを考える． その空間としてLie代数というものが考えられる．

まずは定義からみて，その性質について順々に見ていくことにする．

Lie代数の定義

The Lie algebra is the tangent space at the identity \epsilon

（訳）単位元 \epsilonにおける接空間

接空間

\mathcal{X}(t)を多様体\mathcal{M}上の点のある移動を表す関数とする（tは時刻）．

点 \mathcal{X}における接空間とは，点 \mathcal{X}を通る任意の移動 \mathcal{X}(t)に対する速度ベクトル \mathcal{\dot X} = \mathrm{d} \mathcal{X} / \mathrm{d} tから構成される空間である．


速度ベクトルは，点 \mathcal{X}を通るときの \mathcal{X}(t)の方向（と勢い）を表すものとして直観的には間違いない．

余談

多様体の定義で任意の場所に局所座標系（つまりユークリッド座標系）が描けるということを述べた．点\mathcal{X}にある局所座標系 (U; x_1, x_2, ..., x_m)（各 x_iは局所座標系の軸）があるとして，速度ベクトルは \mathrm{d} \mathcal{X} / \mathrm{d} t = \left(\left.\partial \mathcal{X} / \partial x_1\right|_{t=0}, \left.\partial \mathcal{X} / \partial x_2\right|_{t=0}, ..., \left.\partial \mathcal{X} / \partial x_m\right|_{t=0}\right)と書ける．これがなす空間は実は局所座標系の取り方によらない（「松本幸夫, "多様体の基礎"」）．


\mathcal{X}における接空間は {T_\mathcal{X}} \mathcal{M} と表す．つまり， \mathcal{\dot X} \in T_{\mathcal{X}} \mathcal{M}．

接空間は，点 \mathcal{X}に対するあらゆる速度ベクトルからなる空間であるから，直観的には多様体の点 \mathcal{X}における接平面のように考えることができる．

この記法を使って，Lie代数 \mathfrak{m}を定義すると

Lie代数 \mathfrak{m} \triangleq T_{\epsilon} \mathcal{M}

となる．

つまり，Lie代数とは，最初に示した図のように，Lie群単位元における接平面（のような）空間であると考えることができる．

f:id:shuckle-rbtech:20210524181222p:plain

Lie代数関連の性質

Lie代数には次の性質がある．

  • Lie代数はベクトル空間である．そういうわけで，Lie代数の要素はある基底の組み合わせとして， \mathbb{R}^m空間のベクトルを座標と一対一対応する．

  • 指数写像 \exp: \mathfrak{m} \rightarrow \mathcal{M}と対数写像 \logによって，Lie代数の要素とLie群の要素は一対一対応する．

  • \mathcal{X} における接空間のベクトルは，線形変換によって単位元 \epsilonにおける接空間，つまりLie代数のベクトルへ変換することができる．この変換は随伴（表現）と呼ばれる．

1番目の性質と2つ目の性質は特に重要で，つまりLie群の要素の最適化（e.g., 回転行列などの最適化）を考えたとき，その一対一対応するユークリッド空間 \mathbb{R}^m上での最適化に帰着させることができるということを示している！ これがまさに今やりたいことである．

あと，Lie代数が単位元でしか定義されていないので他の点 \mathcal{X}での最適化はどうするんだと思ったけど，3番目の性質によってすべての最適化を単位元周りの最適化に帰着できるということなんだな．

続く