一、 S O ( 3 ) SO(3) SO(3)上的指數映射
對于 s o ( 3 ) so(3) so(3)中的元素 ϕ \phi ϕ,可計算其模長及方向向量分别為 θ \theta θ和 a a a。經過數學推導(詳見視覺SLAM十四講79頁), S O ( 3 ) SO(3) SO(3)上的指數/對數映射公式如下:
已知 ϕ \phi ϕ,求解 R = exp ( ϕ ∧ ) R{\rm{ = }}\exp (\phi ^ \wedge) R=exp(ϕ∧),即指數映射公式:
已知 R R R,求解 ϕ \phi ϕ,即對數映射公式:
θ = arccos t r ( R ) − 1 2 a = R a \begin{array}{l} \theta = \arccos \frac{{tr(R) - 1}}{2}\\ a = Ra \end{array} θ=arccos2tr(R)−1a=Ra
對于該公式的了解:
1)指數對數映射公式與第三講中旋轉向量與旋轉矩陣的變換公式是一緻的,這說明 s o ( 3 ) so(3) so(3)實際上就是旋轉向量組成的空間。
2)指數映射是一個滿射,但不是一個單射。即每個 R R R都能找到與之對應的 ϕ \phi ϕ,但可能存在多個 ϕ \phi ϕ對應同一個 R R R。最簡單的例子就是旋轉360度和旋轉720度是一樣的。但如果将旋轉角固定在 ± π \pm \pi ±π之間,那麼 S O ( 3 ) SO(3) SO(3)和 s o ( 3 ) so(3) so(3)中的元素是一一對應的。
二、 S E ( 3 ) SE(3) SE(3)上的指數映射
對于 s e ( 3 ) se(3) se(3)上的元素 ξ = [ ρ , ϕ ] \xi = [\rho ,\phi ] ξ=[ρ,ϕ],其中 ϕ = θ a \phi = \theta a ϕ=θa與 S E ( 3 ) SE(3) SE(3)的元素歐氏變換矩陣 T T T,有以下映射關系:
已知 ξ = [ ρ , ϕ ] \xi = [\rho ,\phi ] ξ=[ρ,ϕ],求 T = exp ( ξ ∧ ) {\rm{T = }}\exp (\xi ^\wedge) T=exp(ξ∧),即指數映射:
T = exp ( ξ ∧ ) = [ R t 0 T 1 ] = [ R J ρ 0 T 1 ] {\rm{T = }}\exp (\xi ^\wedge) = \left[ {\begin{matrix} R&t\\ {{0^T}}&1 \end{matrix}} \right] = \left[ {\begin{matrix} R&{J\rho }\\ {{0^T}}&1 \end{matrix}} \right] T=exp(ξ∧)=[R0Tt1]=[R0TJρ1] R = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ R = \cos \theta I + (1 - \cos \theta )a{a^T} + \sin\theta a^\wedge R=cosθI+(1−cosθ)aaT+sinθa∧ J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ J = \frac{{\sin \theta }}{\theta }I + (1 - \frac{{\sin \theta }}{\theta })a{a^T} + \frac{{1 - \cos \theta }}{\theta }{a^ \wedge } J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
已知 T = exp ( ξ ∧ ) = [ R t 0 T 1 ] {\rm{T = }}\exp (\xi ^\wedge) = \left[ {\begin{matrix} R&{t }\\ {{0^T}}&1 \end{matrix}} \right] T=exp(ξ∧)=[R0Tt1],求解 ξ = [ ρ , ϕ ] \xi = [\rho ,\phi ] ξ=[ρ,ϕ],即對數映射:
θ = arccos t r ( R ) − 1 2 a = R a \begin{array}{l} \theta = \arccos \frac{{tr(R) - 1}}{2}\\ a = Ra \end{array} θ=arccos2tr(R)−1a=Ra J = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ J = \frac{{\sin \theta }}{\theta }I + (1 - \frac{{\sin \theta }}{\theta })a{a^T} + \frac{{1 - \cos \theta }}{\theta }{a^ \wedge } J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧ ρ = J − 1 t \rho = {J^{ - 1}}t ρ=J−1t
對于該公式的了解:
1)公式中旋轉部分與 S O ( 3 ) SO(3) SO(3)保持一緻,而 s e ( 3 ) se(3) se(3)的平移部分 ρ \rho ρ到 S E ( 3 ) SE(3) SE(3)的平移部分 t t t則發生了一次以 J J J為系數矩陣的線性變換。