前回の第３回講演会の挨拶
講義 4 リー群とリー代数
第3講では、3次元世界における剛体の運動の記述を紹介しました。その中で、回転の表現に焦点を当てます。ただし、SLAM では、表現に加えて、それらを推定して最適化する必要もあります。SLAMでは姿勢が不明なため、「どのようなカメラ姿勢が現在の観測データに最も適合するのか」といった問題を解決する必要があります。典型的な方法は、これを最適化問題として定式化し、最適なR 、 t 、 R,t 、R 、tで誤差が最小限に抑えられるようにします。
前述したように、回転行列自体は制約されています (直交し、行列式は 1)。最適化変数として使用すると、最適化を困難にする追加の制約が導入されます。リー群とリー代数の間の変換関係を通じて、姿勢推定を制約のない最適化問題に変換し、解法を簡素化したいと考えています。
4.1 リー群とリー代数の基礎
講義 3 では、回転行列と変換行列の定義を紹介しました。その際、3 次元回転行列が特殊直交群 SO(3)を構成し、変換行列が特殊ユークリッド群 SE(3)を構成すると言いました。それらは次のように書かれます:
SO ( 3 ) = { R ∈ R 3 × 3 ∣ RRT = I , det ( R ) = 1 } SO(3) = \{R∈\mathbb{R}^{3 × 3} |RR^T = I、det(R) = 1\}そ( 3 )={
R∈R3 × 3 ∣RRT=私は、d e t ( R )=1 }
SE ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ SO ( 3 ) , t ∈ R 3 } SE(3) = \{T=\begin{bmatrix}R&t \\\\0^T&1\end{bmatrix}∈\mathbb{R}^{4 × 4}|R ∈ SO(3),t ∈ \mathbb{R}^3\}SE ( 3 )={
T=⎣
⎡R0Tt1⎦
⎤∈R4 × 4∣R __∈そ( 3 ) 、t∈R3 }グループ
説明しませんでした。読者は、それが回転行列であっても、変換行列であっても、**それらは加算に閉ざされているわけではないことに気づくはずです。**言い換えると、任意の 2 つの回転行列R 1 、R 2 、R_1、R_2、R1、R2,按照矩阵加法的定义，和不再是一个旋转矩阵：
R 1 + R 2 ∉ S O ( 3 ) , T 1 + T 2 ∉ S E ( 3 ) . R_1 + R_2 ∉ SO(3), \;T_1 + T_2 ∉ SE(3). R1+R2∈/SO(3),T1+T2∈/SE(3).
你也可以说两种矩阵并没有良好定义的加法，或者通常矩阵加法对这两个集合不封闭。相对地，它们只有一种较好的运算：乘法。 S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)关于乘法是封闭的：
R 1 R 2 ∈ S O ( 3 ) , T 1 T 2 ∈ S E ( 3 ) . R_1R_2 ∈ SO(3), \;T_1T_2 ∈ SE(3). R1R2∈SO(3),T1T2∈SE(3).
同时，我们也可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。我们知道，乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个（良好的）运算的集合，我们称之为群。
4.1.1 群
次に、抽象代数の知識を紹介する必要があります。
群 (Group) は、集合と演算の代数構造です。集合をA 、 A、と表します。A 、操作は⋅ 、 \cdot、⋅の場合、グループはG = ( A , ⋅ ) として記録できます。G = (A,\cdot)。G=( A 、⋅ ) 。グループでは、この操作が次の条件を満たす必要があります。
- 封印性：∀ a 1 、 a 2 ∈ A 、 a 1 ⋅ a 2 ∈ A 。\forall a_1,a_2 \in A,\;a_1\cdot a_2\in A.∀a _1、ある2∈あ、ある1⋅ある2∈Ａ．_
- 結合律：∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1,a_2,a_3 \in A,\;( a_1\cdot a_2)\cdot a_3 = a_1\cdot (a_2\cdot a_3)∀a _1、ある2、ある3∈あ、( _1⋅ある2)⋅ある3=ある1⋅( _2⋅ある3)
- 幺元：∃ a 0 ∈ A , s 。と。∀ a ∈ A 、 a 0 ⋅ a = a ⋅ a 0 = a 。\exists a_0 \in A,\;st\;\forall a \in A, \;a_0\cdot a = a \cdot a_0 = a。∃ _0∈あ、s 。と。∀a _∈あ、ある0⋅ある=ある⋅ある0=。_
- 逆：∀ a ∈ A , ∃ a − 1 ∈ A , s 。と。a ⋅ a − 1 = a 0 。\forall a \in A,\;\exist a^{-1} \in A,\;st\;a\cdot a^{-1} = a_0。∀a _∈あ、∃ _− 1∈あ、s 。と。ある⋅ある− 1=ある0。
読者は、上記の 4 つのプロパティを「シールされたユニタリ逆関数」と呼ぶことができます。回転行列と行列の乗算の集合がグループを形成していることは簡単に検証できます。同様に、変換行列と行列の乗算もグループを形成しています (したがって、これらを回転行列グループ、変換行列グループと呼ぶことができます)。その他の一般的なグループには、整数の加算( Z , + ) 、 (\mathbb{Z},+) 、( Z 、+ ) 、 0 を除いた有理数の乗算 (単位は 1)( Q ∖ 0 , ⋅ ) ， (\mathbb{Q} \setminus 0 ,\cdot),( Q∖0 、⋅ )など。行列内の一般的なグループは次のとおりです。
- 一般線形群GL ( n ) GL(n)G L ( n ) 指n × nn × nn×行列乗算用にグループ化されたnの可逆行列。
- 特殊直交群SO ( n ) SO(n)SO ( n ) は、いわゆる回転行列グループです。ここで、SO ( 2 ) SO(2)SO(2)和 S O ( 3 ) SO(3) SO ( 3 )が最も一般的です。
- 特殊ユークリッド群SE ( n ) SE(n)SE ( n ) は前述のnnSE ( 2 ) SE(2)などのn次元ユークリッド変換SE(2)和 S E ( 3 ) SE(3) SE ( 3 )。
群構造は、群に対する操作が良好な特性を持つことを保証します。群理論は、群のさまざまな構造と特性を研究する理論です。群理論に興味のある読者は、現代の代数学の教科書を参照してください。リー群とは連続（滑らか）な性質を持つ群です。整数群Z \mathbb{Z}のようにZのような離散群にはwhileSO ( n ) SO(n)SO(n)和 S E ( n ) SE(n) SE ( n )は実数空間で連続です。剛体は空間内を連続的に移動できるため、それらはすべてリー群であると直感的に想像できます。SO ( 3 ) SO(3)以来SO(3)和 S E ( 3 ) SE(3) SE ( 3 )はカメラの姿勢推定に特に重要であるため、主にこれら 2 つのリー群について説明します。次に、より単純なSO ( 3 ) SO(3)SO ( 3 )はSO (3) SO (3)につながる議論を開始しますSO ( 3 ) so ( 3 ) \mathfrak{so}(3)上のリー代数それで( 3 )
4.1.2 リー代数の導出
任意の回転行列R 、 R、R 、次の条件を満たすことがわかっています。
RRT = I RR^T = IR RT=私
さて、RRと言います。Rはカメラの回転であり、時間の関数として時間の経過とともに連続的に変化します:R ( t )。R(t)。R ( t ) 。まだ回転行列であるため、次のようになります。
R ( t ) R ( t ) T = IR(t)R(t)^T = IR ( t ) R ( t )T=私
方程式の両側で時間に関する微分を取ると、次のようになります。
R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 \.{R}(t)R(t)^T + R(t)\.{R}(t) ^T = 0R˙ (t)R(t)T+R ( t )R˙ (t)T=0
片付ける
R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \.{R}(t)R(t)^T = -(\.{R}(t) R(t)^T)^TR˙ (t)R(t)T=− （R˙ (t)R(t)た）T
R ˙ ( t ) R ( t ) T \.{R}(t)R(t)^T であることが わかります。R˙ (t)R(t)Tは非対称行列です。以前に外積を導入したとき、ベクトルを非対称行列に変換するために ^ 記号が導入されました。同様に、任意の非対称行列についても、対応する唯一のベクトルを見つけて、この演算記号 ˇ \;\check{}ˇは言いました：
a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = aa^{\land} = A = \begin{bmatrix}0&-a_3&a_2\\\\ a_3&0&-a_1\\\\-a_2&a_1&0\end{bmatrix},A^{\lor} = aある∧=あ=⎣ ⎡0ある3− _2− _30ある1ある2− _10⎦ ⎤、あ∨=ある
すると、R ˙ ( t ) R ( t ) T \.{R}(t)R(t)^T なのでR˙ (t)R(t)Tは反対称行列であり、3 次元ベクトルϕ ( t ) ∈ R 3 \phi(t)\in\mathbb{R}^3 をϕ ( t )∈R3 は以下に対応します。
R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ 。\.{R}(t)R(t)^T = \phi(t)^{\land}。R˙ (t)R(t)T=ϕ ( t )∧。
方程式の両辺にR ( t ) 、 R(t) を掛けます。R ( t ) 、 RRによるR は直交行列です。
R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) . \.R(t) = \phi(t)^{\land}R(t) = \begin{bmatrix} 0&-\phi_3&\phi_2\\\\\phi_3&0&-\phi_1\\\\-\phi_2&\phi_1&0 \end{bmatrix}R(t). R˙(t)=ϕ(t)∧R(t)=⎣ ⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦ ⎤R(t).
可以看到，每对旋转矩阵求一次导数，只需左乘一个 ϕ ∧ ( t ) \phi^{\land}(t) ϕ∧(t)矩阵即可。考虑 t 0 = 0 t_0 = 0 t0=0时，设此时旋转矩阵为 R ( 0 ) = I 。 R(0) = I。 R ( 0 )=私。_ 導関数の定義によれば、R ( t ) R(t)t = 0 t = 0におけるR ( t )t=0の周囲で一次テイラー展開を実行します
R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) 。\begin{aligned}R(t) &≈ R(t_0) + \.R{}(t_0)(t - t_0)\\&=I + \phi(t_0)^{\land}(t).\終わり{整列}R ( t )≈R ( t0)+R˙( t0) ( t−t0)=私+ϕ ( t0)∧ (t)。
ϕ \phi であることがわかります。ϕ はRRを反映しますRの導関数プロパティ( 3 ) SO(3)原点付近の接空間上のSO ( 3 ) 。同時にt 0 t_0t0近くで、ϕ \phiにしましょうϕは一定のままですϕ ( t 0 ) = ϕ 0 。\phi(t_0) = \phi_0。ϕ ( t0)=ϕ0。したがって：
R ˙ ( t ) = ϕ ( t 0 ) ∧ R ( t ) = ϕ 0 ∧ R ( t ) 。\.{R}(t) = \phi(t_0)^{\land}R(t) = \phi_0^{\land}R(t)。R˙ (t)=ϕ ( t0)∧ R(t)=ϕ0∧R ( t ) 。
上式はRRに関するものです初期値R ( 0 ) = IR(0) = IによるRの微分方程式R ( 0 )=私は、解決しました
R ( t ) = exp ( ϕ 0 ∧ t ) 。R(t) = \exp{(\phi_0^{\land}t)}。R ( t )=経験値( ϕ0∧t ）。
読者は、上の式が微分方程式と初期値の両方に当てはまることを検証できます。これは、t = 0 では t = 0であることを意味します。t=0に近い場合、回転行列は exp ( ϕ 0 ∧ t ) \exp{(\phi_0^{\land}t)}によって計算できます。経験値( ϕ0∧t )が計算されます。回転行列RRがRと別の反対称行列ϕ 0 ∧ t \phi_0^{\land}tϕ0∧tは指数関係を通じて接続されます。しかし、行列のインデックスは何でしょうか? ここで明確にしておきたい 2 つの質問があります。
- ある瞬間の R が与えられると、R、R 、 ϕ 、 \phiを見つけることができます。ϕ 、 RRを表します。局所導関数関係におけるR。RR付きR はϕ \phiに対応しますϕ とはどういう意味ですかϕ \phiと言います。ϕ はSO ( 3 ) SO(3)に正確に対応しますSO ( 3 ) so ( 3 ) ; \mathfrak{so}(3);に関するリー代数だから( 3 ) ;
- 第二に、特定のベクトルϕ \phiが与えられると、ϕ、行列指数exp ( ϕ ∧ ) \exp{(\phi^{\land})}経験値( ϕ∧ )計算方法は? 逆に、RR がRでは、ϕ を計算する逆演算は可能ですか? \phi?ϕ ?実際、これはまさにリー群とリー代数の間の指数/対数写像です。
以下では、これら 2 つの問題について説明します。
4.1.3 リー代数の定義
すべてのリー群には、対応するリー代数があります。リー代数は、リー群の局所的性質、正確には単位元の周囲の接空間を記述します。一般的なリー代数は次のように定義されます。
リー代数は集合V 、 \mathbb{V} で構成されます。V 、数値フィールドF \mathbb{F}Fと二項演算[ , ] [,][ 、』の構成。以下の特性を満たす場合、 ( V , F , [ , ] ) (\mathbb{V,F,[,]}) と呼ばれます。( V 、ふ、[ 、] )はリー代数であり、g で表されます。\mathfrak{g}。g 。
- 封鎖性 ∀ X , Y ∈ V , [ X , Y ] ∈ V 。\;\forall X,Y \in \mathbb{V},[X,Y]\in\mathbb{V}。∀X 、_Y∈V 、[ X 、はい]∈V. _
- 双線性 ∀ X , Y , Z ∈ V , a , b ∈ F , \;\forall X,Y,Z \in \mathbb{V},a,b \in \mathbb{F},∀X 、_やあ、Z∈V 、、_b∈F ,有
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] 。[aX + bY,Z] = a[X,Z] + b[Y,Z],\;[Z,aX + bY] = a[Z,X] + b[Z,Y]。[ × _+によって、Z ]=a [ X 、Z ]+b [ Y 、Z ] 、[ Z 、× _+byY ]=a [ Z 、× ]+b [ Z 、Y ] 。 - 反射率 ∀ X ∈ V , [ X , X ] = 0。 \;\forall X \in \mathbb{V},[X,X] = 0。∀X _∈V 、[ X 、× ]=0.
- 雅可比等价 ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \;\forall X, Y,Z \in \mathbb{V},[X,[Y,Z]] + [Z,[X,Y]] + [Y,[Z,X]] = 0∀X 、_やあ、Z∈V 、[ X 、[ Y 、Z ]]+[ Z 、[ X 、はい]]+[ Y 、[ Z 、X ]]=0
ここで、二項演算はリー括弧と呼ばれます。表面的には、リー代数には非常に多くの性質が必要です。グループ内の単純な二項演算とは対照的に、リー括弧は 2 つの要素間の差異を表します。結合法則は必要ありませんが、中括弧の後の要素とそれ自体がゼロになるという特性が必要です。例として、3 次元ベクトルR 3 \mathbb{R}^3R3で定義された外積 × はリー括弧なので、 g = ( R 3 , R , × ) \mathfrak{g} = (\mathbb{R}^3,\mathbb{R},×) となります。g=( R3、R 、× ) はリー代数を形成します。
4.1.4 リー代数so ( 3 ) \mathfrak{so}(3)それで( 3 )
前述のϕ 、 \phi、ϕは実際にはリー代数です。ソ (3) ソ(3)SO ( 3 )は R 3 \mathbb{R}^3の定義に対応します。R3上のベクトルをϕ \phiϕ。前の導出によると、各ϕ \phiϕ は非対称行列を生成できます。
Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 。\Phi = \phi^{\land} = \begin{bmatrix} 0&-\phi_3&\phi_2\\\\\\phi_3&0&-\phi_1\\\\-\phi_2&\phi_1&0 \end{bmatrix}\in \mathbb {R}^{3×3}。ファイ=ϕ∧=⎣
⎡0ϕ3− ϕ2− ϕ30ϕ1ϕ2− ϕ10⎦
⎤∈R3 × 3 .
この定義では、2 つのベクトルϕ 1 、ϕ 2 \phi_1,\phi_2ϕ1、ϕ2中括弧
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ 。[\phi_1,\phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^{\lor}。[ p1、ϕ2】=( F1ファイ2−ファイ2ファイ1)∨ .
読者は、この定義の下の中括弧が上記の 4 つのプロパティを満たしているかどうかを確認できます。ベクトルϕ \phiなのでϕと反対称行列ため、 ( 3 ) \mathfrak{so}(3)so ( 3 )の要素は、区別なく 3 次元ベクトルまたは 3 次元反対称行列です。
したがって (3) = {ϕ ∈ R 3, Φ = ϕ ∧ ∈ R 3 × 3}。\mathfrak{so}(3) = \{\phi \in \mathbb{R}^3,\Phi = \phi^{\land} \in \mathbb{R}^{3 × 3}\}。それで( 3 )={ p∈R3、ファイ=ϕ∧∈R3 × 3 }。
一部の書籍ではϕ ^ \hat{\phi}も使用されますϕ^このような表記は反対称を示しますが、意味は同じです。ここまではso ( 3 ) \mathfrak{so}(3) をクリアしました。so ( 3 )の内容。これらは3 次元ベクトルであり、それぞれが非対称行列に対応し、回転行列の導関数を表現するために使用できます。SO ( 3 ) SO(3)に関連していますSO ( 3 )関係は指数マップで与えられます。
R = exp ( ϕ ∧ ) 。R = \exp{(\phi^{\land})}。R=経験値( ϕ∧ )。
指数マッピングについては後で説明します。so ( 3 ) が導入されているので、 \mathfrak{so}(3)、それで( 3 ) 、ところでSE ( 3 ) SE(3)を見てみましょうSE ( 3 )に対応するリー代数
4.1.5 リー代数se ( 3 ) \mathfrak{se}(3)( 3 )付き
SE(3)用SE(3)SE ( 3 )には、対応するリー代数(3) もあります。\mathfrak{se}(3)。せ( 3 ) 。スペースを節約するために、se ( 3 ) \mathfrak{se}(3)せ( 3 )。したがって( 3 ) \mathfrak{so}(3)so ( 3 )は同様です、se ( 3 ) \mathfrak{se}(3)se ( 3 )はR 6 \mathbb{R}^6にありますR6 つのスペース:
se ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ so ( 3 ), ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 }。\mathfrak{se}(3) = \left\{ \xi = \begin{bmatrix} \rho\\\\\phi\end{bmatrix}\in\mathbb{R}^6,\rho\in\mathbb {R}^3,\phi\in\mathfrak{so}(3),\xi^{\land} = \begin{bmatrix}\phi^{\land}&\rho\\\\0^T&0\ {bmatrix}\in\mathbb{R}^{4×4}\right\} を終了します。( 3 )付き=⎩ ⎨ ⎧バツ=⎣ ⎡rϕ⎦ ⎤∈R6、r∈R3、ϕ∈したがって( 3 ) 、バツ∧=⎣ ⎡ϕ∧0Tr0⎦ ⎤∈R4 × 4⎭ ⎬ ⎫。
それぞれのse ( 3 ) \mathfrak{se}(3)を取ります。se ( 3 )の要素はξ 、 \xi、ξ 、これは 6 次元ベクトルです。最初の 3 つの次元は平行移動であり、ρ; \rho;ρ ;最後の 3 つの次元は回転であり、ϕ 、 \phi、ϕ 、これは本質的にそうです ( 3 ) \mathfrak{so}(3)so ( 3 )要素。同時に、 ^ \;\^{}＾記号の意味。atse ( 3 ) \mathfrak{se}(3)se ( 3 )、 ^ \;\^{}^記号は 6 次元ベクトルを 4 次元行列に変換しますが、ここでは反対称性が表現されなくなります。
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 。\xi^{\land} = \begin{bmatrix}\phi^{\land}&\rho\\\\0^T&0\end{bmatrix}\in\mathbb{R}^{4×4}。バツ∧=⎣ ⎡ϕ∧0Tr0⎦ ⎤∈R4 × 4。
私たちは今でも ^ \;\^{}を使用しています^と ˇ \;\check{}ˇ表記は、保持する「ベクトルから行列」および「行列からベクトル」の関係を指します。so ( 3 ) \mathfrak{so}(3)そのための一貫性（3 ） 。依然として 1 対 1 の対応です。読者は単純にse ( 3 ) \mathfrak{se}(3)se ( 3 )は、「翻訳にso ( 3 ) \mathfrak{so}(3)so ( 3 )要素" (ただし、ここではρ \rhoρ は直接の翻訳ではありません）。同様に、リー代数se ( 3 ) \mathfrak{se}(3)se ( 3 )にもso ( 3 ) \mathfrak{so}(3)に似たものがあります。so ( 3 )の Li 括弧
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ 。[\xi_1,\xi_2] = (\xi_1^{\land}\xi_2^{\land} - \xi_2^{\land}\xi_1^{\land})^{\lor}。[ ×1、バツ2】=( ×1∧バツ2∧−バツ2∧バツ1∧)∨ .
読者は、それがリー代数の定義を満たしていることを検証できます。これまでに 2 つの重要なリー代数を見てきましたので、( 3 ) \mathfrak{so}(3)so ( 3 )とse ( 3 ) \mathfrak{se}(3)se(3)了。
4.2 指数マッピングと対数マッピング
4.2.1 SO (3) SO(3)SO上の指数マップ( 3 )
次に 2 番目の質問について考えてみましょう: exp ( ϕ ∧ ) ? \exp{(\phi^{\land})}? を 計算する方法は次のとおりです。経験値( ϕ∧ )?明らかに、それは行列のインデックスであり、リー群およびリー代数では、指数写像と呼ばれます。繰り返しますが、最初にso ( 3 ) \mathfrak{so}(3)について説明します。so ( 3 )の指数写像se ( 3 ) \mathfrak{se}(3)せ( 3 )状況。
任意の行列の指数マップはテイラー展開として書くことができますが、それが収束した場合に限り、結果は依然として行列になります。
exp ( A ) = ∑ n = 0 ∞ 1 n ! あん。\exp(A) =\sum\limits_{n=0}^\infty \frac{1}{n!}A^n。exp ( A )=n = 0∑∞ん！1あn。
同様に、so ( 3 ) \mathfrak{so}(3)についても同様です。so ( 3 ) ϕ , \phi,の任意の要素ϕの場合、次のように指数マップを定義することもできます。
exp ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n \exp(\phi^{\country}) =\sum\limits_{n=0}^\infty \frac{1}{n!}(\phi^{\country})^n。exp ( ϕ∧ )=n = 0∑∞ん！1( ϕ∧ )n。
しかし、行列の無限の累乗を計算したくないため、この定義を直接計算することはできません。以下では、指数マップを計算するための便利な方法を導き出します。ϕ \phi以来ϕは 3 次元ベクトルであり、その係数の長さと方向を定義でき、それぞれθ \thetaθ和 a ， a， aなので、 ϕ = θ a です。\phi = \thetaϕ=θa。这里 a a aは長さ 1 の方向ベクトル、つまり∥ a ∥ = 1 です。\left \Vert と \right \Vert = 1。∥ ∥ _=1 . まず、∧ 、 a^{\land} について、ある∧、次の 2 つのプロパティがあります。
a ∧ a ∧ = [ − a 2 2 − a 3 2 a 1 a 2 a 1 a 3 a 1 a 2 − a 1 2 − a 3 2 a 2 a 3 a 1 a 3 a 2 a 3 − a 1 2 − a 2 2 ] = aa T − I , a^{\land}a^{\land} = \begin{bmatrix}-a^2_2 - a_3^2&a_1a_2&a_1a_3\\\\a_1a_2&-a_1^2-a_3^2&a_2a_3 \\\\a_1a_3&a_2a_3&-a_1^2-a_2^2\end{bmatrix} = aa^T - 私、ある∧ _∧=⎣ ⎡− _22−ある32ある1ある2ある1ある3ある1ある2− _12−ある32ある2ある3ある1ある3ある2ある3− _12−ある22⎦ ⎤=ああ_T−私は、
としても
a ∧ a ∧ a ∧ = a ∧ ( aa T − I ) = − a ∧ 。a^{\land}a^{\land}a^{\land} = a^{\land}(aa^T - I) = -a^{\land}。ある∧ _∧ _∧=ある∧ (あ、T−私）=− _∧。
これら 2 つの式は、 a ∧ a^{\land}の処理を提供します。ある∧高次項のメソッド。指数マップは次のように書くことができます。
exp ( ϕ ∧ ) = exp ( θ a ∧ ) = ∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 ( a ∧ ) 4 + ⋅ ⋅ ⋅ = aa T − a ∧ a ∧ + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 ( a ∧ ) 2 + ⋅ ⋅ ⋅ = aa T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − ⋅ ⋅ ⋅ ) ⏟ sin θ a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − ⋅ ⋅ ⋅ ) ⏟ cos θ a ∧ a ∧ = a ∧ a ∧ + I + sin θ a ∧ − cos θ a ∧ a ∧ = ( 1 − cos θ ) a ∧ a ∧ + I + sin θ a ∧ = cos θ I + ( 1 − cos θ ) aa T + sin θ a ∧ \begin{aligned}\exp(\phi^{\land}) = \exp( \theta a^{\land}) &= \sum\limits_{n = 0}^{\infty}\frac{1}{n!}(\theta a^{\land})^n\\ &= I + \theta a^{\land} + \frac{1}{2!}\theta^{2}a^{\land}a^{\land} + \frac{1}{3!}\theta ^{3}a^{\land}a^{\land}a^{\land} + \frac{1}{4!}\theta^{4}(a^{\land})^4 + ·・・・\\&exp ( ϕ∧ )=exp ( θ a∧ )=n = 0∑∞ん！1(私は∧ )n=私+私は∧+2 ！1私2a_ _∧ _∧+3 !1私3a _∧ _∧ _∧+4 ！1私4 (_∧ )4 +⋅⋅⋅=ああ_T−ある∧ _∧+私は∧+2 ！1私2a_ _∧ _∧−3 !1私3a _∧−4 ！1私4 (_∧ )2 +⋅⋅⋅=ああ_T+ (私は−3 !1私3+5 ！1私5 −⋅⋅⋅)罪私ある∧− ( 1−2 ！1私2+4 ！1私4 −⋅⋅⋅)コス私ある∧ _∧=ある∧ _∧+私+罪私は∧−コス私は∧ _∧=( 1−コス私）a∧ _∧+私+罪私は∧=コスθI _+( 1−コスθ ) a aT+罪私は∧
最後に、おなじみの公式が得られます。
exp ( θ a ∧ ) = cos θ I + ( 1 − cos θ ) aa T + sin θ a ∧ \exp{(\theta a^{\land})} = \cos{\theta}I + (1-\cos{\theta})aa^{T} + \sin{\theta}a^{\land }。経験値(私は∧ )=コスθI _+( 1−コスθ ) a aT+罪私は∧。
講義3の内容を思い出してみると、ロドリゲスの公式と全く同じです。これは、so ( 3 ) \mathfrak{so}(3)であることを示しています。so ( 3 )は実際にはいわゆる回転ベクトル、指数写像はロドリゲスの公式です。それらを通して、 so ( 3 ) \mathfrak{so}(3)とします。so ( 3 )内の任意のベクトルは、 SO ( 3 ) SO(3)内のベクトルに対応します。SOの回転行列( 3 ) 。逆に、対数マップを定義する場合は、SO ( 3 ) SO(3)SO ( 3 )の要素はso ( 3 ) \mathfrak{so}(3)so(3)中：
ϕ = ln ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) nn + 1 ( R − I ) n + 1 ) ∨ 。\phi = \ln{(R)^{\lor}} = \left(\sum\limits_{n = 0}^{\infty}\frac{(-1)^n}{n + 1}(R - I)^{n + 1} \right)^{\lor}。ϕ=ln( R )∨=(n = 0∑∞n + 1( − 1 )ん( R−私）n + 1 )∨。
指数マッピングと同様に、テイラー展開を使用して対数マッピングを直接計算する必要はありません。第 3 回の講義では、回転行列に従って対応するリー代数を計算し、軌跡の性質を利用して回転角度と回転軸をそれぞれ解く方法を紹介しましたが、この方法の方が便利です。
ここで、指数マップの計算を紹介します。それでは、指数写像にはどのような性質があるのでしょうか? どのRRでもR は一意のϕ ? \phi?ϕ ?残念ながら、指数マップは単なる全射であり、射影ではありません (全射、射出、および全射については、チューリングの cat i の記事)。これは、すべてのSO ( 3 ) SO(3)について、SO ( 3 )の要素はso ( 3 ) \mathfrak{so}(3)so ( 3 )要素がそれに対応しますが、複数のso ( 3 ) \mathfrak{so}(3)so ( 3 )の要素は、SO ( 3 ) SO(3)そ( 3 )。少なくとも回転角θ \thetaθ 、 360° 360° が何回転するかがわかります360°和没有转是一样的——它既有周期性。但是，如果我们把旋转角度固定在 ± π ±\pi ±π之间，那么李群和李代数元素是一一对应的。
S O ( 3 ) SO(3) SO(3)与 s o ( 3 ) \mathfrak{so}(3) so(3)的结论似乎在我们的意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即罗德里格斯公式。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。
4.2.2 S E ( 3 ) SE(3) SE(3)上的指数映射
下面介绍 s e ( 3 ) \mathfrak{se}(3) se(3)上的指数映射。如上，形式如下：
exp ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] = Δ [ RJ p 0 T 1 ] = T \begin{aligned}\exp{(\xi^{\land})} &= \begin{bmatrix} \sum\limits_ {n = 0}^{\infty}\frac{1}{n!}(\phi^{\land})^n&\sum\limits_{n = 0}^{\infty}\frac{1}{ (n + 1)!}(\phi^{\land})^n\rho\\\\0^T&1\end{bmatrix}\\\\ &\overset{\Delta}{=}\begin{bmatrix }R&Jp\\\\0^T&1\end{bmatrix} = T \end{aligned}経験値( ×∧ )=⎣ ⎡n = 0∑∞ん！1( ϕ∧ )n0Tn = 0∑∞( n + 1 )!1( ϕ∧ )nρ _1⎦ ⎤=D⎣ ⎡R0T日本_1⎦ ⎤=T
少し辛抱すれば、so ( 3 ) \mathfrak{so}(3)をたどることができます。したがって( 3 )導出、exp \expexp はテイラー展開を実行して問題を導き出します。ϕ = θ a 、 \phi = \theta a としますϕ=θa,其中 a a aが単位ベクトルである場合、
∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n = I + 1 2 ! θ a ∧ + 1 3 ! θ 2 ( a ∧ ) 2 + 1 4 ! θ 3 ( a ∧ ) 3 + 1 5 ! θ 4 ( a ∧ ) 4 ⋅ ⋅ ⋅ = 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋅ ⋅ ⋅ ) ( a ∧ ) + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + ⋅ ⋅ ⋅ ) ( a ∧ ) 2 + I = 1 θ ( 1 − cos θ ) ( a ∧ ) + θ − sin θ θ ( aa T − I ) + I = sin θ θ I + ( 1 − sin θ θ ) aa T + 1 − cos θ θ a ∧ = def J \begin{aligned} \sum\limits_{n = 0}^{\infty}\frac{1}{(n + 1)!}(\phi^{\country})^n&=I + \frac{1 }{2!}\theta a^{\land} + \frac{1}{3!}\theta^{2}(a^{\land})^2 + \frac{1}{4!}\ theta^{3}(a^{\land})^3 +\frac{1}{5!}\theta^{4}(a^{\land})^4 ···\\&= \frac {1}{\theta}\begin{行列}(\frac{1}{2!}\theta^2 - \frac{1}{4!}\theta^4 + ···)\end{行列} (a^{\land}) + \frac{1}{\theta}\begin{行列}(\frac{1}{3!}\theta^3 - \frac{1}{5!}\theta^ 5 + ···)\end{行列}(a^{\land})^2 + I\\& = \frac{1}{\theta}(1-\cos{\theta})(a^{\land}) + \frac{\theta - \sin{\theta}}{\theta}(aa^T - I) + I\\&= \frac{\sin{\theta}}{\theta}I + (1-\frac{\sin\theta}{\theta})aa^{T} + \frac{ 1 - \cos{\theta}}{\theta}a^{\land}\transrated{def}{=}J. \end{整列}n = 0∑∞( n+1 )!1( ϕ∧ )n=私+2 ！1私は∧+3 !1私2 (_∧ )2+4 ！1私3 (_∧ )3+5 ！1私4 (_∧ )4 ⋅⋅⋅=私1(2 ！1私2−4 ！1私4 +⋅⋅⋅)( _∧ )+私1(3 !1私3−5 ！1私5 +⋅⋅⋅)( _∧ )2+私=私1( 1−コス私）（_∧ )+私私−罪私( _ _T−私）+私=私罪私私+( 1−私罪私)ああ_T+私1−コス私ある∧=デフ_ _J .
結果から、ξ \xiξのインデックスは、左上隅のRRをマップしますRはおなじみのSO ( 3 ) SO(3)SO ( 3 )の要素se ( 3 ) \mathfrak{se}(3)se ( 3 )のϕ \phiϕに相当します。そしてJJJは上記の導出によって求められます。
J = sin θ θ I + ( 1 − sin θ θ ) aa T + 1 − cos θ θ a ∧ J = \frac{\sin{theta}}{\theta}I + (1-\frac{\sin\theta}{\theta})aa^{T} + \frac{1 - \cos{\theta } }{\theta}a^{\land}。J=私シン_ _私私+( 1−私シン_ _私)ああ_T+私1 −コス_ _私ある∧。
この式はロドリゲスの式に多少似ていますが、まったく同じではありません。翻訳部分がインデックス マップされた後、 JJが発生していることがわかります。Jは係数行列の線形変換です。同様に、変換行列TT
に従って対数マップを類推することもできますが、T はそう求める( 3 ) \mathfrak{so}(3)so ( 3 )の対応するベクトルには、左上のRRR は回転ベクトルを計算しますが、tt は以下を満たします:
t = J ρ 。t = J\rho.t=J ρ .
JJ のおかげでJ はϕ \phiで与えられますϕが得られるので、ここでρ \rhoρもこの一次方程式から解くことができます。さて、下図に示すように、リー群とリー代数の定義と相互変換関係を明らかにしました。
4.3 リー代数導出と外乱モデル
4.2.1 BCH の公式と近似形式
リー代数を使用する主な動機の 1 つは最適化であり、導関数は非常に必要な情報です。問題を考えてみましょう。SO(3)SO(3)はクリアしましたがSO(3)和 S E ( 3 ) SE(3) SE ( 3 )上のリー群とリー代数の関係、ただしSO ( 3 ) SO(3)上の場合SO ( 3 )で 2 つの行列を乗算すると、リー代数ではso ( 3 ) \mathfrak{so}(3) となります。それで( 3 )何が変わりましたか? 逆に、そうである場合 ( 3 ) \mathfrak{so}(3)so ( 3 ) 、 SO ( 3 ) SO(3)に 2 つのリー代数を追加する場合SO ( 3 )は2 つの行列の積に対応しますか? true の場合、次と同等です。
exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) = exp ( ( ϕ 1 + ϕ 2 ) ∧ ) ? \exp(\phi^{\land}_1)\exp(\phi^{\land}_2) = \exp((\phi_1 + \phi_2)^{\land})?exp ( ϕ1∧)exp ( ϕ2∧)=exp (( ϕ1+ϕ2)∧)?
如果 ϕ 1 , ϕ 2 \phi_1,\phi_2 ϕ1,ϕ2为标量，那么显然该式成立；但此时我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立：
ln ( exp ( A ) exp ( B ) ) = A + B ？ \ln(\exp(A)\exp(B)) = A + B？ ln(exp(A)exp(B))=A+B？
很遗憾，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hausdorff公式(BCH公式)给出。由于其完整形式较复杂，我们只给出其展开式的前几项：
ln ( e x p ( A ) exp ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + ⋅ ⋅ ⋅ \ln(exp(A)\exp(B)) = A + B + \frac{1}{2}[A,B] +\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]] + ··· ln ( e x p ( A )exp ( B ))=あ+B+21[ A 、B ]+121[ A 、[ A 、B ]]−121[ B 、[ A 、B ]] + ⋅⋅⋅
ここで[ ] [\;][]は中括弧です。BCH の公式は、2 つの行列指数の積を処理すると、リー括弧で構成される剰余が生じることを示しています。特に、SO ( 3 ) SO(3)リー代数ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ , \ln(exp(\phi_1^{\land})\exp(\phi_2^{ }land\ln ( e x p ( ϕ1∧)exp ( ϕ2∧) )∨、ϕ 1 \phi_1の場合ϕ1またはϕ 2 \phi_2ϕ2少額の場合は、少額の２次以上の項目は無視して構いません。このとき、BCH は次のような線形近似式になります。
ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 ϕ 1 が小さい場合、 J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 ϕ 2 が小さい場合、 \ln(exp(\phi_1^{\land})\exp(\phi_2^{\land}))^{\lor} ≈\begin{cases} J_l(\phi_2 )^ {-1}\phi_1 + \phi_2& \phi_1 が少量の場合、 \\ J_r(\phi_1)^{-1}\phi_2 + \phi_1& \phi_2 が少量の場合、 \end{cases}ln ( e x p ( ϕ1∧)exp ( ϕ2∧) )∨≈{ J私( ϕ2)− 1p _1+ϕ2Jr( ϕ1)− 1p _2+ϕ1当たりϕ1少量の場合、当たりϕ2少量の場合、
一次近似を例に挙げます。この式は、回転行列R 2 R_2の場合に次のことを示します。R2(リー代数はϕ 2 \phi_2ですϕ2) に小さな回転行列R 1 R_1を乗算したままにします。R1(リー代数はϕ 1 \phi_1ですϕ1)、元のリー代数では、ϕ 2 \phi_2と近似的にみなすことができます。ϕ2これに項J l (ϕ 2 ) − 1 ϕ 1が追加されます。J_l(\phi_2)^{-1}\phi_1。J私( ϕ2)− 1p _1。同様に、2 番目の近似は、小さな変位を右に乗算する場合を表します。したがって、BCH 近似では、リー代数は左乗算近似と右乗算近似の 2 種類に分けられ、使用する際には左乗算モデルを使用するか右乗算モデルを使用するかに注意する必要があります。
この本では例として左乗算を使用します。左乗算 BCH 近似ヤコビアンJ l J_lJ私実際、これは式 (4.27) の内容です。
J l = J = sin θ θ I + ( 1 − sin θ θ ) aa T + 1 − cos θ θ a ∧ J_l = J = \frac{\sin\theta}{\theta}I + (1 - \frac{\sin\theta}{\theta})aa^T + \frac{1 - \;\cos\theta} {\theta}a^{\land}。J私=J=私シン_ _私私+( 1−私シン_ _私)ああ_T+私1 −コス_ _私ある∧。
その逆は
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) aa T − θ 2 a ∧ 。J_l^{-1} = \frac{\theta}{2}\cot\frac{\theta}{2}I + (1 - \frac{\theta}{2}\cot\frac{\theta}{ 2})aa^T - \frac{\theta}{2}a^{\land}。J私− 1=2私ベビーベッド2私私+( 1−2私ベビーベッド2私)ああ_T−2私ある∧。
正しい乗算ヤコビアンは、独立変数の負の符号を取るだけで済みます。
J r ( ϕ ) = J l ( − ϕ ) 。J_r(\phi) = J_l(-\phi)。Jr( ϕ )=J私( − ϕ ) 。
このようにして、リー群の乗算とリー代数の加算の間の関係について話すことができます。
読者の便宜のために、BCH 近似の意味を再度説明します。ある回転R 、 R、R 、対応するリー代数はϕ です。\ファイ。ϕ 。Δ R 、 \Delta Rとして示される小さな回転を左に掛けますΔ R 、対応するリー代数はΔ ϕ です。\デルタ\ファイ。Δϕ._ _ _ 次に、リー群上で得られる結果は、Δ R ⋅ R 、 \Delta R·R、Δ R ⋅R 、およびリー代数では、BCH 近似によれば、J l − 1 ( ϕ ) Δ ϕ + ϕ です。J_l^{-1}(\phi)\Delta\phi + \phi.J私− 1( ϕ ) D ϕ+ϕ 。組み合わせると、次のように簡単に書くことができます。
exp ( Δ ϕ ∧ ) exp ( ϕ ∧ ) = exp ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ )。\exp(\Delta\phi^{\land})\exp(\phi^{\land}) = \exp((\phi + J_l^{-1}(\phi)\Delta\phi)^{\土地}）。exp ( Δ ϕ∧ )exp ( ϕ∧ )=exp (( ϕ+J私− 1( ϕ ) D ϕ )∧ )。
逆に、リー代数で加算を実行する場合、 a ϕ \phiとします。ϕプラスΔϕ， \Delta\phi，Δ ϕ 、 aϕ \phiϕプラスΔϕ， \デルタ \phi，Δ ϕの場合、リー群上の左右のヤコビアンの乗算として近似できます。
exp ( ( ϕ + Δ ϕ ) ∧ ) ) = exp ( ( J l Δ ϕ ) ∧ ) exp ( ϕ ∧ ) = exp ( ϕ ∧ ) exp ( ( J r Δ ϕ ) ∧ ) 。\exp((\phi + \Delta\phi)^{\land})) = \exp((J_l\Delta\phi)^{\land})\exp(\phi^{\land}) = \exp (\phi^{\land})\exp((J_r\Delta\phi)^{\land})。exp (( ϕ+D ϕ )∧ ))=exp (( J私D ϕ )∧ )exp ( ϕ∧ )=exp ( ϕ∧ )exp (( JrD ϕ )∧ )。
これは、後でリー代数の微積分を行うための理論的基礎を提供します。同様に、SE ( 3 ) 、 SE(3) については、SE ( 3 )にも同様の BCH 近似があります。
exp ( Δ ξ ∧ ) exp ( ξ ∧ ) ≈ exp ( ( J l − 1 Δ ξ + ξ ) ∧ ) , \exp(\Delta\xi^{\land})\exp(\xi^{ \land}) ≈ \exp((\mathcal{J}_l^{-1}\Delta\xi + \xi)^{\land}),exp ( D ξ∧ )経験値( ξ∧ )≈exp (( J私− 1D ×+× ）∧ )、
exp ( ξ ∧ ) exp ( Δ ξ ∧ ) ≈ exp ( ( J r − 1 Δ ξ + ξ ) ∧ ) 。\exp(\xi^{\land})\exp(\Delta\xi^{\land}) ≈ \exp((\mathcal{J}_r^{-1}\Delta\xi + \xi)^{ \土地}）。経験値( ξ∧ )exp ( D ξ∧ )≈exp (( Jr− 1D ×+× ）∧ )。
ここで $\mathcal{J_l} の形式はより複雑で、6 × 6 6 × 6です。6×6マトリックス。計算ではヤコビアンを使用しなかったため、その実際の形式はここでは省略されます。
4.3.2 SO(3) 上のリー代数の導出
リー代数を含む関数とリー代数の導出の問題について議論しましょう。この質問には強力な実践的な背景があります。SLAM では、 SO ( 3 ) SO(3)によって与えられるカメラの位置と姿勢を推定したいとします。SO ( 3 )またはSE(3) SE(3)の回転行列SE ( 3 )の変換行列で説明されますある瞬間の小さな大根のポーズをTに設定すると良いかもしれませんT.T._ _ ppで世界座標を観測しますpの点は観測データz を生成します。z.z 。次に、座標変換関係から次のようになります。
z = T p + w 。z = Tp + w。z=TP+w 。
其中 w w wはランダムノイズです。その存在のおかげで、zz多くの場合、 zがz = T pz = Tp正確にz=TP関係。したがって、通常は理想的な観測値と実際のデータの間の誤差を計算します。
e = z − T p 。e = z - Tp.e=z−TP 。
NN があるとします。このようなランドマーク ポイントと観測点がN 個あるため、NN上記のN 個の式。したがって、大根の姿勢を推定することは、最適なT、T、全体的な誤差が最小化されるようにT :
min TJ ( T ) = ∑ i = 1 N ∥ zi − T pi ∥ 2 2 。\min\limits_TJ(T) = \sum\limits^N_{i = 1}\Vert z_i - Tp_i\Vert_2^2。T分J ( T )=i = 1∑N∥z _私は−Tp _私は∥22。
この問題を解決するには、目的関数JJを計算する必要があります。変換行列TTに対するJTの派生語。具体的なアルゴリズムについては後ほど残しておきます。ここで重要なのは、ポーズに関連した関数を構築し、その後、現在の推定値を調整するためにポーズに関するその関数の導関数について議論することが多いということです。**ただし、SO ( 3 ) 、SO(3)、SO ( 3 ) 、 SE ( 3 ) SE(3)SE ( 3 )には明確に定義された追加機能はなくTTを取るならTは最適化を処理するために通常の行列として扱われるため、制約する必要があります。リー代数の観点から見ると、リー代数はベクトルで構成され、優れた加算演算を備えているため、導出の問題を解決するためにリー代数を使用する方法は 2 つあります。
- リー代数を使用して態度を表現し、リー代数加算に従ってリー代数を導出します。
- リー群に小さな外乱を左右に乗算して外乱を導き出し、これを左外乱モデルおよび右外乱モデルと呼びます。
1 つ目の方法はリー代数の導関数モデルに対応し、2 つ目の方法は外乱モデルに対応します。これら 2 つのアプローチの類似点と相違点については、以下で説明します。
4.3.3 リー代数の導出
まず、SO ( 3 ) SO(3)を考えます。SOのケース( 3 ) 。空間点ppがあるとします。pを回転してR p Rpを取得しますRp . ここで、回転後の回転に関する点の座標の導関数を計算するには、非公式に次のように書きます。
∂ ( R p ) ∂ ( R ) \frac{\partial(Rp)}{\partial(R)}∂ ( R )∂ ( Rp )
SO ( 3 ) SO(3)以来SO ( 3 )には加算がないため、この導関数は導関数の定義によって計算できません。RRさせましょうRに対応するリー代数はϕ 、 \phi、ϕ 、次の計算に移ります。
∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ 。\frac{\partial(\exp(\phi^{\land})p)}{\partial\phi}。∂ ϕ∂ ( e x p ( ϕ∧ )p)。
導関数の定義によれば、次のようになります。
∂ ( exp ( ϕ ∧ ) p ) ∂ ϕ = lim δ ϕ → 0 exp ( ( ϕ + δ ϕ ) ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 ( J l δ ϕ ) ∧ exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 − ( exp ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l \begin{aligned} \frac{\partial(\exp(\phi^{\land})p)}{\partial\phi}&= \lim\limits_{\delta\phi\rightarrow0}\frac{\exp((\phi + \delta\phi)^{\land})p - \exp(\phi^{\land})p}{\ delta\phi}\\\\&= \lim\limits_{\delta\phi\rightarrow0}\frac{\exp((J_l\delta\phi)^{\land})\exp(\phi^{\land })p - \exp(\phi^{\land})p}{\delta\phi}\\\\&= \lim\limits_{\delta\phi\rightarrow0}\frac{(I +(J_l\ delta\phi)^{\land})\exp(\phi^{\land})p - \exp(\phi^{\land})p}{\delta\phi}\\\\&= \lim\limits_{\delta\phi\rightarrow0}\frac{(J_l\delta\phi)^{\land} \exp(\phi^{\land})p}{\delta\phi}\\\ \&= \lim\limits_{\delta\phi\rightarrow0}\frac{-(\exp(\phi^{\land})p)^{\land}J_l\delta\phi }{\delta\phi} = -(Rp)^{\land}J_l \end{整列}∂ ϕ∂ ( exp ( ϕ∧ )p)=δ ϕ → 0リムd ϕexp (( ϕ+d ϕ )∧ )p−exp ( ϕ∧ )p=δ ϕ → 0リムd ϕexp (( J私d ϕ )∧ )exp ( ϕ∧ )p−exp ( ϕ∧ )p=δ ϕ → 0リムd ϕ(私+( J私d ϕ )∧ )exp ( ϕ∧ )p−exp ( ϕ∧ )p=δ ϕ → 0リムd ϕ( J私d ϕ )∧exp ( ϕ∧ )p=δ ϕ → 0リムd ϕ− ( exp ( ϕ∧ )p)∧ J私d ϕ=− ( Rp )∧ J私
2行目の近似はBCH線形近似、3行目の近似は高次項を破棄したテイラー展開後の近似(極限がかかっているので等号を書けます)、4行目から5行目は反対称記号を次のようにみなしています。外積、交換後の数値を変更します。したがって、リー代数に関する回転点の導関数を導出します。
∂ ( R p ) ∂ ϕ = ( − R p ) ∧ J l 。\frac{\partial(Rp)}{\partial\phi} = (-Rp)^{\land}J_l。∂ ϕ∂ ( Rp )=( − Rp )∧ J私。
ただし、J l 、J_l には依然として複雑な形式が存在するため、J私、それを計算する必要はありません。以下で説明する摂動モデルは、導関数を計算する簡単な方法を提供します。
4.3.4 外乱モデル（左乗算）
別の導出方法はRRです。R は外乱Δ R \Delta RΔ R、外乱に対する結果の変化率を見てください。この外乱は左または右で乗算することができ、最終的な結果は若干異なります。左の外乱を例に考えてみましょう。左外乱をΔ R \Delta RΔR はリー代数φに対応します。\varphi.φ . 次に、φ \varphiφ導関数、つまり
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ 。\frac{\partial(Rp)}{\partial\varphi} = \lim\limits_{\varphi\rightarrow0}\frac{\exp(\varphi^{\land})\exp(\phi^{\land} )p\;-\;\exp(\phi^{\land})p}{\varphi}。∂ φ∂ ( Rp )=φ → 0リムファイe x p ( f∧ )e x p ( ϕ∧ )p−e x p ( ϕ∧ )p。
この式の導関数は上記よりも簡単です。
∂ ( R p ) ∂ φ = lim φ → 0 exp ( φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 ( I + φ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 φ ∧ R p φ = lim φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ 。\begin{aligned}\frac{\partial(Rp)}{\partial\varphi} &= \lim\limits_{\varphi\rightarrow0}\frac{\exp(\varphi^{\land})\exp(\ phi^{\land})p\;-\;\exp(\phi^{\land})p}{\varphi}\\\\&= \lim\limits_{\varphi\rightarrow0}\frac{( I + \varphi^{\land})\exp(\phi^{\land})p\;-\;\exp(\phi^{\land})p}{\varphi}\\\\&= \lim\limits_{\varphi\rightarrow0}\frac{\varphi^{\land}Rp}{\varphi} = \lim\limits_{\varphi\rightarrow0}\frac{-(Rp)^{\land}\ varphi}{\varphi} = -(Rp)^{\land}。\end{整列}∂ φ∂ ( Rp )=φ → 0リムファイexp ( f∧ )exp ( ϕ∧ )p−exp ( ϕ∧ )p=φ → 0リムファイ(私+ファイ∧ )exp ( ϕ∧ )p−exp ( ϕ∧ )p=φ → 0リムファイファイ∧ Rp=φ → 0リムファイ− ( Rp )∧ φ=− ( Rp )∧ .
リー代数の直接導出と比較すると、ヤコビアンJ l J_lが省略されている ことがわかります。J私計算。これにより、摂動モデルがより実用的になります。読者はここでの微分演算を理解する必要があります。これは姿勢推定において非常に重要です。
4.3.5 SE 上のリー代数の導出(3)
最後に、 SE ( 3 ) SE(3) を与えます。SE ( 3 )の摂動モデルと直接リー代数の導出はもう導入されません。ある空間点ppp は変換TTT (リー代数に対応するのはξ \xiξ )、T p TpTP。さて、TTTには外乱Δ T = exp ( δ ξ ∧ ) , \Delta T = \exp(\delta\xi^{\land}) が乗算されたままになります。ΔT _=exp ( δ ξ∧ )として、外乱項のリー代数をδ ξ = [ δ ρ , δ ϕ ] T 、 \delta\xi = [\delta\rho,\delta\phi]^{T} として設定します。dx _=[ d r 、d ϕ ]Tの場合:
∂ ( T p ) ∂ δ ξ = lim δ ξ → 0 exp ( δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 ( I + δ ξ ∧ ) exp ( ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 δ ξ ∧ exp ( ξ ∧ ) p δ ξ = lim φ → 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ R p + t 1 ] δ ξ = lim φ → 0 [ δ ϕ ∧ ( R p + t ) + δ ρ 0 T ] [ δ ρ , δ ϕ ] T = [ I − ( R p + t ) ∧ 0 T 0 T ] = def ( T p ) ⨀ 。\begin{aligned}\frac{\partial(Tp)}{\partial\delta\xi} &= \lim\limits_{\delta\xi\rightarrow0}\frac{\exp(\delta\xi^{\land })\exp(\xi^{\land})p\;-\;\exp(\xi^{\land})p}{\delta\xi}\\\\&= \lim\limits_{\ delta\xi\rightarrow0}\frac{(I + \delta\xi^{\land})\exp(\xi^{\land})p\;-\;\exp(\xi^{\land}) p}{\delta\xi}\\\\&= \lim\limits_{\delta\xi\rightarrow0}\frac{\delta\xi^{\land}\exp(\xi^{\land})p }{\delta\xi}\\\\ & = \lim\limits_{\varphi\rightarrow0}\frac{\begin{bmatrix}\delta\phi^{\land}&\delta\rho\\\\0^T&0\end{bmatrix}\begin{bmatrix} Rp + t\\\\1\end{bmatrix}}{\delta\xi}\\\\ &= \lim\limits_{\varphi\rightarrow0}\frac{\begin{bmatrix}\delta\phi^{ \land}(Rp + t) + \delta\rho\\\\0^T\end{bmatrix}}{[\delta\rho,\delta\phi]^{T}}= \begin{bmatrix}I& -(Rp + t)^{\land}\\\\0^T&0^T\end{bmatrix} \overset{def}{=}(Tp)^{\bigodot}。\end{整列}∂ δ ξ∂ ( Tp )=δ ξ → 0リムdx _exp ( δ ξ∧ )経験値( ξ∧ )p−経験値( ξ∧ )p=δ ξ → 0リムdx _(私+dx _∧ )経験値( ξ∧ )p−経験値( ξ∧ )p=δ ξ → 0リムdx _dx _∧経験値( ξ∧ )p=φ → 0リムdx _⎣ ⎡d ϕ∧0Tドル_0⎦ ⎤⎣ ⎡ルピア+t1⎦ ⎤=φ → 0リム[ d r 、d ϕ ]T⎣ ⎡d ϕ∧ (Rp+t )+ドル_0T⎦ ⎤=⎣ ⎡私0T− ( Rp+t )∧0T⎦ ⎤=デフ_ _( TP )⨀ .
最終結果を演算子⨀ ^{\bigodot}として定義します。⨀、同次座標の空間点を4 × 6 4×64×6マトリックス。この式で説明する必要があるのは、 a 、 b 、 x 、 ya、b、x、y と仮定した場合の行列導出の順序です。、_b 、× 、y は両方とも列ベクトルであるため、この記述方法には次の規則があります。
d [ ab ] d [ xy ] = ( d [ a , b ] T d [ xy ] ) = [ godxdbdxdadydbdy ] T = [ godxdadydbdxdbdy ] \frac{d\begin{bmatrix}\\\\a\\\\b \\\\\end{bmatrix}}{d\begin{bmatrix}\\\\x\\\\y\\\\\end{bmatrix}} = \left( \frac{d[a,b] ^T}{d\begin{bmatrix}\\\\x\\\\y\\\\\end{bmatrix}} \right) = \begin{bmatrix}\frac{da}{dx}&\frac {db}{dx}\\\\\frac{da}{dy}&\frac{db}{dy}\end{bmatrix}^T =\begin{bmatrix}\frac{da}{dx}&\ frac{da}{dy}\\\\\frac{db}{dx}&\frac{db}{dy}\end{bmatrix}d⎣ ⎡バツy⎦ ⎤d⎣ ⎡あるb⎦ ⎤=⎝ ⎛d⎣ ⎡バツy⎦ ⎤d [ a , b ]T⎠ ⎞=⎣ ⎡dx _だあ_y _だあ_dx _db _y _db _⎦ ⎤T=⎣ ⎡dx _だあ_dx _db _y _だあ_y _db _⎦ ⎤
ここまでリー群とリー代数の微分操作を紹介してきました。次の章では、この知識を応用して実際的な問題を解決していきます。リー群とリー代数のいくつかの重要な数学的性質は、読者のための演習として残されています。
4.4 実践: ソフォス
4.4.1 Sophusの基本的な使い方
リー代数の入門知識をカバーしたので、次は実際に学習した内容を実践して確認し、定着させていきます。プログラム内でリー代数を操作する方法について説明します。講義 3 では、Eigen は幾何学モジュールを提供しますが、リー代数はサポートしないことを見ました。優れたリー代数ライブラリは、Strasdat によって管理されている Sophus ライブラリです。Sophus ライブラリは、この章で主に説明するSO(3) SO(3)をサポートしています。SO(3)和 S E ( 3 ) ， SE(3)， SE ( 3 )には、さらに 2 次元運動SO ( 2 )、SE ( 2 ) SO(2)、SE(2)そ( 2 ) 、SE ( 2 )と同様の変換Simの内容(3)。これは、Eigen に基づいて直接開発されているため、追加の依存関係をインストールする必要はありません。Sophus の初期バージョンでは、倍精度リー群/リー代数クラスのみが提供されていました。後続のバージョンはテンプレート クラスに変更されました。テンプレートクラスのSophusでは精度の異なるリー群/リー代数を使用することができますが、同時に使用の難易度が高くなります。この本では、Sophus ライブラリとテンプレートを使用します。
詳細については、slambook/ch4/useSophus.cppSophus ライブラリのデモを行うSO(3) SO(3)SO(3)和 S E ( 3 ) SE(3) SE ( 3 )操作:
操作結果は次のとおりです。
上記のプログラムは 2 つの部分の内容を示しています。前半はSO(3)SO(3)SO ( 3 )の操作SE(3) の操作。SE(3)。SE ( 3 ) 。SO ( 3 ) 、 SE ( 3 ) SO(3),SE(3) を構築する方法を示します。そ( 3 ) 、SE ( 3 )オブジェクト、それらに対して指数および対数マッピングを実行する方法、および更新量がわかっている場合にリー群要素を更新する方法。この講義の内容を理解していれば、このプログラムは難しくないはずです。コンパイルするには、次のコード行を CMakeLists.txt に追加する必要があります。
find_package( Sophus REQUIRED)
include_directories( ${
Sophus_INCLUDE_DIRS})
add_executable( useSophus useSophus.cpp)
find_package コマンドは、特定のライブラリのヘッダー ファイルとライブラリ ファイルを検索するために cmake によって提供される命令です。cmake がそれを見つけることができれば、ヘッダー ファイルとライブラリ ファイルが配置されているディレクトリの変数が提供されます。Sophus の場合は、Sophus_INCLUDE_DIRS です。Eigen などのテンプレートベースの Sophus ライブラリには、ヘッダー ファイルのみが含まれ、ソース ファイルは含まれません。彼らによると、Sophus ライブラリを独自の cmake プロジェクトにインポートできるとのことです。
4.4.2 例: 軌道誤差の評価
実際のエンジニアリングでは、アルゴリズムの精度を評価するために、アルゴリズムの推定軌道と実際の軌道との差を評価する必要があることがよくあります。多くの場合、実際の軌道は何らかの高精度システムによって取得され、推定軌道は評価対象のアルゴリズムによって計算されます。第3回ではファイルに保存されているトラックを表示する方法を説明しましたが、今回は2つのトラックの誤差を計算する方法を考えます。推定軌道T esti , i T_{esti,i}を考えます。Tそれは、私です。そして実際の軌道T gt , i , T_{gt,i},Tgt 、 i _,其中 i = 1 , ⋅ ⋅ ⋅ ， N ， i = 1, ···， N， 私=1 、⋅⋅⋅, Nの場合、それらの違いを説明するためにいくつかのエラー メトリックを定義できます。
エラー指標には多くの種類がありますが、一般的なものは次の形式の絶対軌道誤差 (絶対軌道誤差、ATE)
ATE all = 1 N ∑ i = 1 N ∥ log ( T gt , i − 1 T esti , i ) ∨ ∥ 2 2 , ATE_{all} = \sqrt{\frac{1}{N}\sum\limits_ {i = 1}^N\Vert \log(T^{-1}_{gt,i}T_{esti,i})^{\lor}\Vert^2_2},アテ_ _すべて_=N1i = 1∑N∥ログ( T _gt 、i _− 1Tそれは、私です。)∨ ∥22、
これは実際には、各ポーズのリー代数の二乗平均平方根誤差 (RMSE)です。この誤差は、2 つの軌道の回転誤差と並進誤差を特徴付けることができます。同時に、一部の文献では翻訳誤差のみが考慮されているため、絶対的な翻訳誤差 (Avergae Translational Error) を定義できます。
ATE trans = 1 N ∑ i = 1 N ∥ trans ( T gt , i − 1 T esti , i ) ∥ 2 2 , ATE_{trans} = \sqrt{\frac{1}{N}\sum\limits_{i = 1}^N\Vert trans(T^{-1}_{gt,i}T_{esti,i})\Vert^2_2},アテ_ _トランス_ _ _=N1i = 1∑N∥トランス( T _ _ _gt 、i _− 1Tそれは、私です。) ∥22、
このうちtransは変数の括弧内の翻訳部分を取ることを意味します。軌道全体で見ると、回転に誤差が生じると、その後の軌道には平行移動にも誤差が生じるため、どちらの指標も実際に適用できます。
これに加えて、相対誤差も定義できます。たとえば、iiについて考えてみましょう。i時刻到達i + Δ ti + \Delta t私+時間Δtでの動き、相対姿勢誤差は次のように定義できます。
RPE all = 1 N − Δ t ∑ i = 1 N − Δ t ∥ log ( ( T gt , i − 1 T gt , i + Δ t ) − 1 ( T esti , i − 1 T esti , i + Δ t ) ) ∨ ∥ 2 2 、 RPE_{all} = \sqrt{\frac{1}{N - \Delta t}\sum\limits_{i = 1}^{N - \Delta t}\Vert \log( (T^{-1}_{gt,i}T_{gt,i + \Delta t})^{-1}(T_{esti,i}^{-1}T_{esti,i + \Delta t }))^{\lor}\Vert^2_2}、RP Eすべて_=N − Δ t1i = 1∑N − Δ t∥log ( ( Tgt 、i _− 1Tg t , i + Δ t)− 1 (T私は、私です− 1Tes t i , i + Δ t) )∨ ∥22、
同様に、翻訳部分のみを取得できます。
RPE trans = 1 N − Δ t ∑ i = 1 N − Δ t ∥ trans ( ( T gt , i − 1 T gt , i + Δ t ) − 1 ( Testi , i − 1 T esti , i + Δ t ) ) ∥ 2 2 , RPE_{trans} = \sqrt{\frac{1}{N - \Delta t}\sum\limits_{i = 1}^{N - \Delta t}\Vert trans((T^ {-1}_{gt,i}T_{gt,i + \Delta t})^{-1}(T_{esti,i}^{-1}T_{esti,i + \Delta t})) \Vert^2_2}、RP Eトランス_ _ _=N − Δ t1i = 1∑N − Δ t∥トランス( ( T _ _gt 、i _− 1Tg t , i + Δ t)− 1 (T私は、私です− 1Tes t i , i + Δ t)) ∥22、
Sophus ライブラリを使用すると、計算のこの部分を簡単に実装できます。次に、絶対軌道誤差の計算を示します。この例では、groundtruth.txt とestimated.txt という 2 つの軌跡があり、次のコードはこれら 2 つの軌跡を読み取り、誤差を計算し、3D ウィンドウに表示します。簡潔にするために、軌道を描画するコードは省略されています。講義 3 でも同様のことを行いました。
コード参照slambook/ch4/example/trajectoryError.cpp
コードを正常に実行するプロセスも、通常とは異なる紆余曲折があり、次の 3 つのステップに分かれています。
- まず、 fmtライブラリがありません。新しいバージョンの Sophus には Pangolin が必要なだけでなく、fmt をインストールする必要があるため、インストールするだけです。
- インストールして実行しても、エラーが報告されます。Baidu
fmt/core.h:1711:3: error: static assertion failed: Cannot format an argument.は、これはバージョンの問題だと言いました。fmt 8.1.1を使用することをお勧めします。fmt 8.1.1の次のバージョンでは、以前のバージョンを削除する必要はありません。前の手順を繰り返すだけです。 - インストール後、次の 2 つの文を CMakeLists.txt に忘れずに追加してください。cmake にこのライブラリを含めて、プログラムを実行できるようにします。
find_package(FMT REQUIRED)
target_link_libraries(trajectoryError fmt::fmt)
- すると、ファイルアドレスが間違っており、2つのtxtファイルのディレクトリが見つからないことがわかります。次に、ファイルのディレクトリを変更する必要がありますが、
このコードを実行するのに約 1 時間かかりました~!!
結果は次のようになります:
プログラムの出力は 2.20728 です。もちろん、回転部分を除いて、平行移動部分の誤差のみを計算することも可能です。この例に関する限り、軌道の時間調整や外部パラメータの推定など、いくつかの前処理タスクを実際に実行するのに役立ちましたが、これらの内容についてはまだ言及されていませんが、今後の研究で言及される予定です。
4.5 * 類似の変換とリー代数のグループ
最後に、単眼視で使用される類似度群S im ( 3 ) Sim(3)について触れたいと思います。S im ( 3 )、および対応するリー代数sim (3)。\mathfrak{sim}(3)。シム( 3 ) 。
単眼スケールの不確実性についてはすでに導入しました。単眼SLAMでSE(3) SE(3)SE ( 3 )はポーズを表します。スケールの不確実性とスケール ドリフトにより、SLAM プロセス全体のスケールが変化します。これはSE ( 3 ) SE (3)SE ( 3 )には反映されませんしたがって、単眼の場合は、通常、スケール係数を明示的に表現します。数学用語では、空間内の点pppは、カメラ座標系では、ユークリッド変換の代わりに同様の変換
p ' = [ s R t 0 T 1 ] p = s R p + t 。p^{'} = \begin{bmatrix}sR&t\\\\0^T&1\end{bmatrix}p = sRp + t。p』=⎣ ⎡R _0Tt1⎦ ⎤p=s Rp+と。
相似変換では、スケールsssが表現されています。ppにも作用するpの 3 つの座標の上pppは 1 回スケーリングされます。SO( 3 )、SE( 3 ) SO(3)、SE(3)付きSO ( 3 )とSE ( 3 )は類似しており、類似の変換は行列乗算のグループも形成します。これは、類似の変換グループ Sim(3) と呼ばれます。
S im ( 3 ) = { S = [ s R t 0 T 1 ] ∈ R 4 × 4 } Sim(3) = \begin{Bmatrix} S = \begin{bmatrix}sR&t\\\\0^T &1\ end{bmatrix}\in \mathbb{R}^{4 × 4}\end{Bmatrix}シム( 3 ) _=⎩ ⎨ ⎧S=⎣ ⎡R _0Tt1⎦ ⎤∈R4 × 4⎭ ⎬ ⎫
同様に、Sim(3) にも、対応するリー代数、指数写像、対数写像などがあります。リー代数シミュレーション ( 3 ) \mathfrak{sim}(3)sim ( 3 )要素は 7 次元ベクトルζ \zetaz。最初の 6 次元はse ( 3 ) \mathfrak{se}(3)se ( 3 )は同じですが、最後に余分な項σ \sigmas。
sim ( 3 ) = { ξ ∣ ξ = [ ρ ϕ σ ] ∈ R 7 , ξ ∧ = [ σ I + ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{sim}(3) = \begin {Bmatrix} \zeta|\zeta = \begin{bmatrix}\rho\\\\\\phi\\\\\\sigma\end{bmatrix}\in \mathbb{R}^7,\zeta^{\ land} = \begin{bmatrix}\sigma I + \phi^{\land}&\rho\\\\0^T &0\end{bmatrix}\in\mathbb{R}^{4 × 4}\end {Bマトリックス}シム( 3 )=⎩ ⎨ ⎧ゼータ∣ゼータ=⎣ ⎡rϕp⎦ ⎤∈R7、g∧=⎣ ⎡σI _+ϕ∧0Tr0⎦ ⎤∈R4 × 4⎭ ⎬ ⎫
se ( 3 ) \mathfrak{se}(3) よりも優れていますse ( 3 )にはもう 1 つの項σ \sigmaσ。Sim(3) をsim ( 3 ) \mathfrak{sim}(3)sim ( 3 )は依然として指数写像と対数写像です。インデックスのマッピング先
exp ( ζ ∧ ) = [ e σ exp ( ϕ ∧ ) J s ρ 0 T \exp(\zeta^{\country}) = \begin{bmatrix} e^{\sigma}\exp(\phi^{\country})&J_s\rho\\\\0^T&1\end{bmatrix}。経験値( g∧ )=⎣ ⎡epexp ( ϕ∧ )0TJsr1⎦ ⎤。
その中でも、J s J_sJsの形で
J s = e σ − 1 σ I + σ e σ sin θ + ( 1 − e σ cos θ ) θ σ 2 + θ 2 a ∧ + ( e σ − 1 σ − ( e σ cos θ − ) σ + (および σ sin θ ) θ σ 2 + θ 2 ) a ∧ a ∧ \begin{aligned}J_s &=\frac{e^\sigma - 1}{\sigma}I + \frac{\sigma e^\sigma \sin\theta + (1- e^\sigma \cos\theta) \theta}{\sigma^2 + \theta^2}a^{\land}\\&\;\;\;\;+\left( \frac{e^\sigma - 1}{\sigma} - \frac{(e^\sigma \cos\theta - 1 )\sigma + (e^\sigma \sin\theta)\theta}{\sigma^2 + \theta^2} \right)a^{\land }a^{\country}.\end{aligned}Js=pep−1私+p2+私2eでp罪私+( 1−epコス私）私ある∧+(pep−1−p2+私2( epコス私−1 ) p+( ep罪私）私)ある∧ _∧ .
指数写像を通じて、リー代数とリー群の関係を見つけることができます。リー代数ζ 、 \zeta の場合、ξ 、およびリー群との対応は次のとおりです。
s = e σ 、R = exp ( ϕ ∧ ) 、t = J s ρ s = e^\sigma,R=\exp(\phi^{\land}),t=J_s\rho。s=e、 _R=exp ( ϕ∧ )、t=Jsr 。
回転部分は SO(3) と一致します。翻訳部分、se ( 3 ) \mathfrak{se}(3)se ( 3 ) では、ヤコビアンJ 、 \mathcal{J} を乗算する必要があります。J 、および相似変換のヤコビアンはより複雑です。スケール係数については、リー群のss をs はリー代数のσ \sigmaσの指数関数
Sim(3) の BCH 近似は SE(3) と似ています。ポイントについて話し合うことができますppp は相似変換S p SpSp 、 SSに対する相対値Sの派生語。同様に差分モデルと外乱モデルの２通りがあり、外乱モデルは比較的単純です。導出プロセスを省略し、摂動モデルの結果を直接与えます。S p Sp Sp としましょうSp ( ζ ∧ ) , \exp(\zeta^{\land}) の左側にある小さな擾乱経験値( g∧ )、S p Spを求めます。外乱に関するSpの導関数なぜなら、S p SpSp は4 次元の同次座標ζ \zetaξは 7 次元ベクトルなので、導関数は4×7 4×74×7のヤコビアン便宜上、S p SpSpの最初の 3 次元合成ベクトルはq 、q、qの場合:
∂ S p ∂ ζ = [ I − q ∧ q 0 T 0 T 0 ] \frac{\partial{Sp}}{\partial\zeta} = \begin{bmatrix}I&-q^{\land}&q\\ \\0^T&0^T&0\end{bmatrix}∂ ζ∂スプ=⎣ ⎡私0T−q _∧0Tq0⎦ ⎤
4.6 概要
この講義では、リー群 SO(3) と SE(3)、およびそれらに対応するリー代数so ( 3 ) \mathfrak{so}(3) を紹介します。ソ( 3 )とセ ( 3 )。\mathfrak{se}(3)。せ( 3 ) 。これらにポーズの表現と変形を導入し、BCH の線形近似を通じてポーズを摂動させて導出することができます。これは、後で説明する姿勢の最適化のための理論的基礎を築きます。これは、対応する誤差を減らすために、特定の姿勢の推定値を頻繁に調整する必要があるためです。ポーズを調整および更新する方法を理解した後でのみ、次に進むことができます。
なお、回転はリー代数以外にも四元数やオイラー角などでも表現できますが、その後の処理が面倒になります。実際のアプリケーションでは、SE(3) の代わりに SO(3) と変換を使用することもでき、これにより一部のヤコビ計算を回避できます。