视觉SLAM十四讲学习记录 第四讲(习题)
课后习题
1. 验证SO(3)、SE(3)和Sim(3)关于乘法成群。
首先对以上三种可以得知，矩阵乘法必然满足结合律，即先计算前两个矩阵乘法还是后两个矩阵乘法是肯定一样的。
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ( R ) = 1 } SO(3) = \{{R\in\mathbb{R}^{3×3}|RR^ T = I,\det(R) = 1\}} SO(3)={R∈R3×3∣RRT=I,det(R)=1}
封闭性：两次旋转矩阵连乘等于一个新的旋转矩阵（代数角度可以验证， R 1 ∗ R 2 R_1*R_2 R1∗R2仍然是行列式为1的正交阵）
幺元： I ⋅ R = R ⋅ I = R I·R = R·I = R I⋅R=R⋅I=R
逆： det ( R ) = 1 ⇒ R \det(R) = 1 \Rightarrow R det(R)=1⇒R可逆 同时 R − 1 = R T R^{-1} = R^T R−1=RT
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 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∈SO(3),t∈R3}
封闭性：两次欧式变换连乘等于一个新的欧式变换（代数角度验证也可， T 1 ∗ T 2 T_1*T_2 T1∗T2仍满足上述形式）
幺元： I ⋅ T = T ⋅ I = T I·T = T·I = T I⋅T=T⋅I=T，表示不进行旋转和平移
逆： T = [ R t 0 T 1 ] T=\begin{bmatrix}R&t\\\\0^T&1\end{bmatrix} T=⎣ ⎡R0Tt1⎦ ⎤根据分块矩阵求逆公式 [ A C 0 B ] − 1 = [ A − 1 − A − 1 C B − 1 0 B − 1 ] \begin{bmatrix}A&C\\\\0&B\end{bmatrix}^{-1} = \begin{bmatrix}A^{-1}&-A^{-1}CB^{-1}\\\\0&B^{-1}\end{bmatrix} ⎣ ⎡A0CB⎦ ⎤−1=⎣ ⎡A−10−A−1CB−1B−1⎦ ⎤
T − 1 = [ R − 1 − R − 1 t 0 T 1 ] ∈ S E ( 3 ) \;\;\;\;\;\;T ^{-1} = \begin{bmatrix}R^{-1}&-R^{-1}t\\\\0^T&1\end{bmatrix}\in SE(3) T−1=⎣ ⎡R−10T−R−1t1⎦ ⎤∈SE(3)
S i m ( 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} Sim(3)=⎩ ⎨ ⎧S=⎣ ⎡sR0Tt1⎦ ⎤∈R4×4⎭ ⎬ ⎫
封闭性：两次相似变换连乘等于一个新的相似变换（代数角度验证也可， S 1 ∗ S 2 S_1*S_2 S1∗S2仍满足上述形式）
幺元：单位矩阵，表示不进行旋转、平移、缩放
逆： a ′ = s R a + t a^{'} = sRa + t a′=sRa+t逆变换为 a = ( R T a ′ − R T t ) / s a = (R^Ta^{'} - R^{T}t)/s a=(RTa′−RTt)/s
2. 验证 ( R 3 , R , × ) (\mathbb{R}^3,\mathbb{R},×) (R3,R,×)构成李代数
封闭性：集合中任意两个元素通过李括号运算得到的结果仍属于集合，三维向量叉乘三维向量，得到的结果仍然是三维向量
双线性：乘法分配律
自反性： R × R = ∣ R ∣ ∣ R ∣ sin θ = 0 ( θ = 0 ° ) R × R = \vert R \vert\vert R \vert \sin{\theta} = 0(\theta = 0^{°}) R×R=∣R∣∣R∣sinθ=0(θ=0°)
雅可比等价：可以通过向量的拉格朗日公式：
a × ( b × c ) = b ( a ⋅ c ) − c ( a ⋅ b ) a × (b × c) = b(a·c) - c(a·b) a×(b×c)=b(a⋅c)−c(a⋅b)
代入下式：
a × ( b × c ) + c × ( a × b ) + b × ( c × a ) a × (b × c) + c × (a × b) + b × (c × a) a×(b×c)+c×(a×b)+b×(c×a)
可得结果为0
3. 验证 s o ( 3 ) \mathfrak{so}(3) so(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)满足李代数要求的性质
李代数 s o ( 3 ) \mathfrak{so}(3) so(3)的集合为3维向量 ϕ \phi ϕ。其对应反对称矩阵 Φ \Phi Φ,两个向量 ϕ 1 , ϕ 2 \phi_1,\phi_2 ϕ1,ϕ2的李括号为 [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \begin{bmatrix}\phi_1,\phi_2\end{bmatrix} = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^{\lor} [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
封闭性： Φ 1 Φ 2 − Φ 2 Φ 1 = − ( Φ 1 Φ 2 − Φ 2 Φ 1 ) T \Phi_1\Phi_2 - \Phi_2\Phi_1 = -(\Phi_1\Phi_2 - \Phi_2\Phi_1)^T Φ1Φ2−Φ2Φ1=−(Φ1Φ2−Φ2Φ1)T仍为反对称矩阵，可对应 ϕ \phi ϕ
双线性： [ a ϕ 1 + b ϕ 2 , ϕ 3 ] = a [ ϕ 1 , ϕ 3 ] + b [ ϕ 2 , ϕ 3 ] [a\phi_1 + b\phi_2 ,\phi_3] = a[\phi_1,\phi_3] + b[\phi_2,\phi_3] [aϕ1+bϕ2,ϕ3]=a[ϕ1,ϕ3]+b[ϕ2,ϕ3]
即证
[ ( a Φ 1 + b Φ 2 ) ⋅ Φ 3 − Φ 3 ⋅ ( a Φ 1 + b Φ 2 ) ] ∨ = a ( Φ 1 Φ 3 − Φ 3 Φ 1 ) ∨ + b ( Φ 2 Φ 3 − Φ 3 Φ 2 ) ∨ = ( a Φ 1 Φ 3 − a Φ 3 Φ 1 + b Φ 2 Φ 3 − b Φ 3 Φ 2 ) ∨ \begin{aligned}[(a\Phi_1 + b\Phi_2)·\Phi_3 - \Phi_3·(a\Phi_1 + b\Phi_2)]^{\lor} &= a(\Phi_1\Phi_3 - \Phi_3\Phi_1)^{\lor} + b(\Phi_2\Phi_3 - \Phi_3\Phi_2)^{\lor} \\&= (a\Phi_1\Phi_3 - a\Phi_3\Phi_1 + b\Phi_2\Phi_3 - b\Phi_3\Phi_2)^{\lor}\end{aligned} [(aΦ1+bΦ2)⋅Φ3−Φ3⋅(aΦ1+bΦ2)]∨=a(Φ1Φ3−Φ3Φ1)∨+b(Φ2Φ3−Φ3Φ2)∨=(aΦ1Φ3−aΦ3Φ1+bΦ2Φ3−bΦ3Φ2)∨
自反性： [ ϕ 1 , ϕ 1 ] = ( Φ 1 Φ 1 − Φ 1 Φ 1 ) ∨ = 0 [\phi_1,\phi_1] = (\Phi_1\Phi_1 - \Phi_1\Phi_1)^{\lor} = 0 [ϕ1,ϕ1]=(Φ1Φ1−Φ1Φ1)∨=0
雅可比等价： [ ϕ 1 , [ ϕ 2 , ϕ 3 ] ] + [ ϕ 3 , [ ϕ 1 , ϕ 2 ] ] + [ ϕ 2 , [ ϕ 3 , ϕ 1 ] ] = 0 [\phi_1,[\phi_2,\phi_3]] + [\phi_3,[\phi_1,\phi_2]] + [\phi_2,[\phi_3,\phi_1]] = 0 [ϕ1,[ϕ2,ϕ3]]+[ϕ3,[ϕ1,ϕ2]]+[ϕ2,[ϕ3,ϕ1]]=0
[ Φ 1 , ( Φ 2 Φ 3 − Φ 3 Φ 2 ) ∨ ] = [ Φ 1 ( Φ 2 Φ 3 − Φ 3 Φ 2 ) − ( Φ 2 Φ 3 − Φ 3 Φ 2 ) Φ 1 ] ∨ = ( Φ 1 Φ 2 Φ 3 − Φ 1 Φ 3 Φ 2 − Φ 2 Φ 3 Φ 1 + Φ 3 Φ 2 Φ 1 ) ∨ = A ∨ \begin{aligned}[\Phi_1,(\Phi_2\Phi_3 - \Phi_3\Phi_2)^{\lor}] &=[\Phi_1(\Phi_2\Phi_3 - \Phi_3\Phi_2) - (\Phi_2\Phi_3 - \Phi_3\Phi_2)\Phi_1]^{\lor}\\\\ &=(\Phi_1\Phi_2\Phi_3 - \Phi_1\Phi_3\Phi_2 - \Phi_2\Phi_3\Phi_1 + \Phi_3\Phi_2\Phi_1)^{\lor} = A^{\lor} \end{aligned} [Φ1,(Φ2Φ3−Φ3Φ2)∨]=[Φ1(Φ2Φ3−Φ3Φ2)−(Φ2Φ3−Φ3Φ2)Φ1]∨=(Φ1Φ2Φ3−Φ1Φ3Φ2−Φ2Φ3Φ1+Φ3Φ2Φ1)∨=A∨
[ Φ 3 , [ Φ 1 , Φ 2 ] ] = ( Φ 3 Φ 1 Φ 2 − Φ 3 Φ 2 Φ 1 − Φ 1 Φ 2 Φ 3 + Φ 2 Φ 1 Φ 3 ) ∨ = B ∨ [\Phi_3,[\Phi_1,\Phi_2]] = (\Phi_3\Phi_1\Phi_2 - \Phi_3\Phi_2\Phi_1 - \Phi_1\Phi_2\Phi_3 + \Phi_2\Phi_1\Phi_3)^{\lor} = B^{\lor} [Φ3,[Φ1,Φ2]]=(Φ3Φ1Φ2−Φ3Φ2Φ1−Φ1Φ2Φ3+Φ2Φ1Φ3)∨=B∨
[ Φ 2 , [ Φ 3 , Φ 1 ] ] = ( Φ 2 Φ 3 Φ 1 − Φ 2 Φ 1 Φ 3 − Φ 3 Φ 1 Φ 2 + Φ 1 Φ 3 Φ 2 ) ∨ = C ∨ [\Phi_2,[\Phi_3,\Phi_1]] = (\Phi_2\Phi_3\Phi_1 - \Phi_2\Phi_1\Phi_3 - \Phi_3\Phi_1\Phi_2 + \Phi_1\Phi_3\Phi_2)^{\lor} = C^{\lor} [Φ2,[Φ3,Φ1]]=(Φ2Φ3Φ1−Φ2Φ1Φ3−Φ3Φ1Φ2+Φ1Φ3Φ2)∨=C∨
( A ∨ + B ∨ + C ∨ ) = ( A + B + C ) ∨ = 0 (A^{\lor} + B^{\lor} + C^{\lor}) = (A + B + C)^{\lor} = 0 (A∨+B∨+C∨)=(A+B+C)∨=0
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 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\end{bmatrix}\in\mathbb{R}^{4×4}\right\}. se(3)=⎩ ⎨ ⎧ξ=⎣ ⎡ρϕ⎦ ⎤∈R6,ρ∈R3,ϕ∈so(3),ξ∧=⎣ ⎡ϕ∧0Tρ0⎦ ⎤∈R4×4⎭ ⎬ ⎫.
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ [\xi_1,\xi_2] = (\xi_1^{\land}\xi_2^{\land} - \xi_2^{\land}\xi^{\land}_1)^{\lor} [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨
封闭性：即证 ( ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ ) ∧ ((\xi_1^{\land}\xi_2^{\land} - \xi_2^{\land}\xi^{\land}_1)^{\lor})^\land ((ξ1∧ξ2∧−ξ2∧ξ1∧)∨)∧仍满足 [ ϕ ∧ ρ 0 T 0 ] \begin{bmatrix}\phi^{\land}&\rho\\\\0^T&0\end{bmatrix} ⎣ ⎡ϕ∧0Tρ0⎦ ⎤的形式
[ Φ 1 ρ 1 0 T 0 ] [ Φ 2 ρ 2 0 T 0 ] − [ Φ 2 ρ 2 0 T 0 ] [ Φ 1 ρ 1 0 T 0 ] = [ Φ 1 Φ 2 − Φ 2 Φ 1 Φ 1 ρ 2 − Φ 2 ρ 1 0 T 0 ] = [ [ ϕ 1 , ϕ 2 ] ρ 3 0 T 0 ] \begin{aligned}\begin{bmatrix}\Phi_1&\rho_1\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_2&\rho_2\\\\0^T&0\end{bmatrix} - \begin{bmatrix}\Phi_2&\rho_2\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_1&\rho_1\\\\0^T&0\end{bmatrix} &=\begin{bmatrix}\Phi_1\Phi_2 - \Phi_2\Phi_1&\Phi_1\rho_2 - \Phi_2\rho_1\\\\0^T&0\end{bmatrix} \\\\&=\begin{bmatrix}[\phi_1,\phi_2]&\rho_3\\\\0^T&0\end{bmatrix} \end{aligned} ⎣ ⎡Φ10Tρ10⎦ ⎤⎣ ⎡Φ20Tρ20⎦ ⎤−⎣ ⎡Φ20Tρ20⎦ ⎤⎣ ⎡Φ10Tρ10⎦ ⎤=⎣ ⎡Φ1Φ2−Φ2Φ10TΦ1ρ2−Φ2ρ10⎦ ⎤=⎣ ⎡[ϕ1,ϕ2]0Tρ30⎦ ⎤
即 [ ξ 1 , ξ 2 ] = [ Φ 1 ρ 2 − Φ 2 ρ 1 , [ ϕ 1 , ϕ 2 ] ] [\xi_1,\xi_2] = \begin{bmatrix}\Phi_1\rho_2-\Phi_2\rho_1\\ ,\\ [\phi_1,\phi_2]\end{bmatrix} [ξ1,ξ2]=⎣ ⎡Φ1ρ2−Φ2ρ1,[ϕ1,ϕ2]⎦ ⎤
双线性： [ a ξ 1 + b ξ 2 , ξ 3 ] = a [ ξ 1 , ξ 3 ] + b [ ξ 2 , ξ 3 ] [a\xi_1 + b\xi_2,\xi_3] = a[\xi_1,\xi_3] + b[\xi_2,\xi_3] [aξ1+bξ2,ξ3]=a[ξ1,ξ3]+b[ξ2,ξ3]
[ a Φ 1 + b Φ 2 a ρ 1 + b ρ 2 0 T 0 ] [ Φ 3 ρ 3 0 T 0 ] − [ Φ 3 ρ 3 0 T 0 ] [ a Φ 1 + b Φ 2 a ρ 1 + b ρ 2 0 T 0 ] = a [ Φ 1 ρ 1 0 T 0 ] [ Φ 3 ρ 3 0 T 0 ] − a [ Φ 3 ρ 3 0 T 0 ] [ Φ 1 ρ 1 0 T 0 ] + b [ Φ 2 ρ 2 0 T 0 ] [ Φ 3 ρ 3 0 T 0 ] − b [ Φ 3 ρ 3 0 T 0 ] [ Φ 2 ρ 2 0 T 0 ] = a [ ξ 1 , ξ 3 ] + b [ ξ 2 , ξ 3 ] \begin{aligned}&\begin{bmatrix}a\Phi_1 + b\Phi_2&a\rho_1 + b\rho_2\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix} - \begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix}\begin{bmatrix}a\Phi_1 + b\Phi_2&a\rho_1 + b\rho_2\\\\0^T&0\end{bmatrix} \\\\&=a\begin{bmatrix}\Phi_1&\rho_1\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix} - a\begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_1&\rho_1\\\\0^T&0\end{bmatrix} + b\begin{bmatrix}\Phi_2&\rho_2\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix} - b\begin{bmatrix}\Phi_3&\rho_3\\\\0^T&0\end{bmatrix}\begin{bmatrix}\Phi_2&\rho_2\\\\0^T&0\end{bmatrix} \\\\&= a[\xi_1,\xi_3] + b[\xi_2,\xi_3] \end{aligned} ⎣ ⎡aΦ1+bΦ20Taρ1+bρ20⎦ ⎤⎣ ⎡Φ30Tρ30⎦ ⎤−⎣ ⎡Φ30Tρ30⎦ ⎤⎣ ⎡aΦ1+bΦ20Taρ1+bρ20⎦ ⎤=a⎣ ⎡Φ10Tρ10⎦ ⎤⎣ ⎡Φ30Tρ30⎦ ⎤−a⎣ ⎡Φ30Tρ30⎦ ⎤⎣ ⎡Φ10Tρ10⎦ ⎤+b⎣ ⎡Φ20Tρ20⎦ ⎤⎣ ⎡Φ30Tρ30⎦ ⎤−b⎣ ⎡Φ30Tρ30⎦ ⎤⎣ ⎡Φ20Tρ20⎦ ⎤=a[ξ1,ξ3]+b[ξ2,ξ3]
自反性： [ ξ 1 , ξ 1 ] = ( ξ 1 ∧ ξ 1 ∧ − ξ 1 ∧ ξ 1 ∧ ) ∨ = 0 ∨ [\xi_1,\xi_1] = (\xi_1^{\land}\xi_1^{\land} - \xi_1^{\land}\xi^{\land}_1)^{\lor} = 0^{\lor} [ξ1,ξ1]=(ξ1∧ξ1∧−ξ1∧ξ1∧)∨=0∨
雅可比等价：
[ ξ 1 , [ ξ 2 , ξ 3 ] ] = [ ξ 1 , [ Φ 2 ρ 3 − Φ 3 ρ 2 ( ϕ 2 , ϕ 3 ) ] ] = [ Φ 1 ( Φ 2 ρ 3 − Φ 3 ρ 2 ) − [ ϕ 2 , ϕ 3 ] ρ 1 [ ϕ 1 , [ ϕ 2 , ϕ 3 ] ] ] = [ Φ 1 Φ 2 ρ 3 − Φ 1 Φ 3 ρ 2 − Φ 2 Φ 3 ρ 1 + Φ 3 Φ 2 ρ 1 [ ϕ 1 , [ ϕ 2 , ϕ 3 ] ] ] \begin{aligned}[\xi_1,[\xi_2,\xi_3]] &= \begin{bmatrix}\xi_1,\begin{bmatrix}\Phi_2\rho_3-\Phi_3\rho_2\\\\(\phi_2,\phi_3)\end{bmatrix}\end{bmatrix} \\\\&=\begin{bmatrix}\Phi_1(\Phi_2\rho_3 - \Phi_3\rho_2)-[\phi_2,\phi_3]\rho_1\\\\ [\phi_1,[\phi_2,\phi_3]]\end{bmatrix} \\\\&=\begin{bmatrix}\Phi_1\Phi_2\rho_3 - \Phi_1\Phi_3\rho_2 - \Phi_2\Phi_3\rho_1 + \Phi_3\Phi_2\rho_1\\\\ [\phi_1,[\phi_2,\phi_3]]\end{bmatrix} \end{aligned} [ξ1,[ξ2,ξ3]]=⎣ ⎡ξ1,⎣ ⎡Φ2ρ3−Φ3ρ2(ϕ2,ϕ3)⎦ ⎤⎦ ⎤=⎣ ⎡Φ1(Φ2ρ3−Φ3ρ2)−[ϕ2,ϕ3]ρ1[ϕ1,[ϕ2,ϕ3]]⎦ ⎤=⎣ ⎡Φ1Φ2ρ3−Φ1Φ3ρ2−Φ2Φ3ρ1+Φ3Φ2ρ1[ϕ1,[ϕ2,ϕ3]]⎦ ⎤
因此最后三项相加可互相抵消（上半部分代数和为0，下半部分有 s o ( 3 ) \mathfrak{so}(3) so(3)雅可比等价可知为0）
4. 验证性质（4.20）和（4.21）
(4.20)
a ∧ a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = [ − 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 ] = [ a 1 2 a 1 a 2 a 1 a 3 a 1 a 2 a 2 2 a 2 a 3 a 1 a 3 a 2 a 3 a 3 2 ] − [ a 1 2 + a 2 2 + a 3 2 0 0 0 a 1 2 + a 2 2 + a 3 2 0 0 0 a 1 2 + a 2 2 + a 3 2 ] = a a T − I \begin{aligned}a^\land a^\land &= \begin{bmatrix}0&-a_3&a_2\\\\a_3&0&-a_1\\\\-a_2&a_1&0\end{bmatrix}\begin{bmatrix}0&-a_3&a_2\\\\a_3&0&-a_1\\\\-a_2&a_1&0\end{bmatrix} \\\\&= \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} \\\\&= \begin{bmatrix}a_1^2&a_1a_2&a_1a_3\\\\a_1a_2&a_2^2&a_2a_3\\\\a_1a_3&a_2a_3&a_3^2\end{bmatrix} - \begin{bmatrix}a_1^2 + a_2^2 + a_3^2&0&0\\\\0&a_1^2 + a_2^2 + a_3^2&0\\\\0&0&a_1^2 + a_2^2 + a_3^2\end{bmatrix} \\\\&= aa^T - I \end{aligned} a∧a∧=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤=⎣ ⎡−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22⎦ ⎤=⎣ ⎡a12a1a2a1a3a1a2a22a2a3a1a3a2a3a32⎦ ⎤−⎣ ⎡a12+a22+a32000a12+a22+a32000a12+a22+a32⎦ ⎤=aaT−I
(4.21)
a ∧ a a T = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ a 1 a 2 a 3 ] = 0 \begin{aligned}a^{\land}aa^{T} = \begin{bmatrix}0&-a_3&a_2\\\\a_3&0&-a_1\\\\-a_2&a_1&0\end{bmatrix}\begin{bmatrix}a_1\\\\a_2\\\\a_3\end{bmatrix} = 0 \end{aligned} a∧aaT=⎣ ⎡0a3−a2−a30a1a2−a10⎦ ⎤⎣ ⎡a1a2a3⎦ ⎤=0
a ∧ a ∧ a ∧ = a ∧ ( a a T − I ) = a ∧ a a T − a ∧ = − a ∧ \begin{aligned} a^{\land}a^{\land}a^{\land} &= a^{\land}(aa^{T} - I) \\\\&= a^{\land}aa^{T} - a^{\land} \\\\&= -a^{\land} \end{aligned} a∧a∧a∧=a∧(aaT−I)=a∧aaT−a∧=−a∧
5. 证明：
R p ∧ R T = ( R p ) ∧ ⇔ R p ∧ = ( R p ) ∧ R . ⇔ ∀ u ∈ R 3 R p ∧ u = ( R p ) ∧ R u ⇔ ∀ u ∈ R 3 R ( p × u ) = ( R p ) × ( R u ) \begin{aligned}&\;\;\;\;\;Rp^\land R^T = (Rp)^\land \\\\ &\Leftrightarrow Rp^\land = (Rp)^\land R. \\\\ &\Leftrightarrow \forall u\in R^3 \;\;\;\;\;Rp^\land u = (Rp)^\land Ru \\\\ &\Leftrightarrow \forall u\in R^3 \;\;\;\;\;R(p × u) = (Rp)×(Ru) \end{aligned} Rp∧RT=(Rp)∧⇔Rp∧=(Rp)∧R.⇔∀u∈R3Rp∧u=(Rp)∧Ru⇔∀u∈R3R(p×u)=(Rp)×(Ru)
最后一式利用向量叉乘的旋转变换不变性
从三维几何的角度来理解： v , u v,u v,u 是任意两个三维向量， ( v × u ) (v × u) (v×u)是一个和 v , u v, u v,u 都垂直、大小为 ∣ v ∣ ∣ u ∣ s i n ( u , v ) ∣v∣∣u∣sin(u,v) ∣v∣∣u∣sin(u,v) 的三维向量；将 v , u , v × u v,u,v×u v,u,v×u 三个向量都经过同一个旋转，它们的相对位姿和模长都不会改变，所以 ( R v ) (Rv) (Rv) 和 ( R u ) (Ru) (Ru) 的叉乘仍是 R ( v × u ) R(v×u) R(v×u)。
6. 证明：
R exp ( p ∧ ) R T = exp ( ( R p ∧ ) ) . R\exp(p^\land)R^T = \exp((Rp^\land)). Rexp(p∧)RT=exp((Rp∧)).
该式称为 S O ( 3 ) SO(3) SO(3)上的伴随性质。同样地，在 S E ( 3 ) SE(3) SE(3)上也有伴随性质：
T exp ( ξ ∧ ) T − 1 = exp ( ( A d ( T ) ξ ) ∧ ) , T\exp(\xi^\land)T^{-1} = \exp((Ad(T)\xi)^\land), Texp(ξ∧)T−1=exp((Ad(T)ξ)∧),
其中：
A d ( T ) = [ R t ∧ R 0 R ] . Ad(T) =\begin{bmatrix}R&t^\land R\\\\0&R\end{bmatrix}. Ad(T)=⎣ ⎡R0t∧RR⎦ ⎤.
exp ( ( R p ) ∧ ) = exp ( R p ∧ R T ) = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T ) n = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T ) ⋅ ( R p ∧ R T ) ⋅ ⋅ ⋅ ( R p ∧ R T ) ( R T R = I ) = ∑ n = 0 ∞ 1 n ! R ( p ∧ ) n R T = R ⋅ ( ∑ n = 0 ∞ 1 n ! ( p ∧ ) n ) R T = R ⋅ exp ( p ∧ ) R T \begin{aligned}\exp((Rp)^\land) &= \exp(Rp^\land R^T) \\\\&=\sum\limits_{n = 0}^{\infty}\frac{1}{n!}(Rp^\land R^T)^n \\\\&=\sum\limits_{n = 0}^{\infty}\frac{1}{n!}(Rp^\land R^T)·(Rp^\land R^T)···(Rp^\land R^T) \\\\&\;\;\;\;\;(R^T R = I) \\\\&=\sum\limits_{n = 0}^{\infty}\frac{1}{n!}R(p^\land)^n R^T \\\\&=R·(\sum\limits_{n = 0}^{\infty}\frac{1}{n!}(p^\land)^n)R^T \\\\&=R·\exp(p^\land)R^T \end{aligned} exp((Rp)∧)=exp(Rp∧RT)=n=0∑∞n!1(Rp∧RT)n=n=0∑∞n!1(Rp∧RT)⋅(Rp∧RT)⋅⋅⋅(Rp∧RT)(RTR=I)=n=0∑∞n!1R(p∧)nRT=R⋅(n=0∑∞n!1(p∧)n)RT=R⋅exp(p∧)RT
7.依照左扰动的推导，推导SO(3)和SE(3)在右扰动下的导数。
SO(3)右扰动下的导数
设右扰动 Δ R \Delta R ΔR对应的李代数为 ϕ \phi ϕ,对 ϕ \phi ϕ求导
∂ ( R p ) ∂ φ = lim φ → 0 exp ( ϕ ∧ ) exp ( φ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 exp ( ϕ ∧ ) ( I + φ ∧ ) p − exp ( ϕ ∧ ) p φ = lim φ → 0 exp ( ϕ ∧ ) ⋅ φ ∧ p φ = lim φ → 0 R φ ∧ p φ = lim φ → 0 ( R φ ) ∧ ⋅ ( R p ) φ = lim φ → 0 − ( R p ) ∧ ⋅ ( R φ ) φ = − ( R p ) ′ ⋅ R \begin{aligned}\frac{\partial(Rp)}{\partial\varphi}&=\lim_{\varphi\rightarrow0}\frac{\exp(\phi^\land)\exp(\varphi^\land)p - \exp(\phi^\land)p}{\varphi} \\\\&=\lim_{\varphi\rightarrow0}\frac{\exp(\phi^\land)(I + \varphi^\land)p - \exp(\phi^\land)p}{\varphi} \\\\&=\lim_{\varphi\rightarrow0}\frac{\exp(\phi^\land)·\varphi^\land p}{\varphi} \\\\&=\lim_{\varphi\rightarrow0}\frac{R\varphi^\land p}{\varphi} \\\\&=\lim_{\varphi\rightarrow0}\frac{(R\varphi)^\land·(Rp)}{\varphi} \\\\&=\lim_{\varphi\rightarrow0}\frac{-(Rp)^\land·(R\varphi)}{\varphi} \\\\&=-(Rp)^{'}·R \end{aligned} ∂φ∂(Rp)=φ→0limφexp(ϕ∧)exp(φ∧)p−exp(ϕ∧)p=φ→0limφexp(ϕ∧)(I+φ∧)p−exp(ϕ∧)p=φ→0limφexp(ϕ∧)⋅φ∧p=φ→0limφRφ∧p=φ→0limφ(Rφ)∧⋅(Rp)=φ→0limφ−(Rp)∧⋅(Rφ)=−(Rp)′⋅R
SE(3)右扰动下的导数,设右扰动 Δ T \Delta T ΔT对应的李代数为 δ ξ = [ δ ρ , δ ϕ ] T \delta\xi = [\delta \rho,\delta\phi]^T δξ=[δρ,δϕ]T
∂ ( T p ) ∂ δ ξ = lim δ ξ → 0 exp ( ξ ∧ ) exp ( δ ξ ∧ ) p − exp ( ξ ∧ ) p φ = lim δ ξ → 0 exp ( ξ ∧ ) ( I + δ ξ ∧ ) p − exp ( ξ ∧ ) p δ ξ = lim δ ξ → 0 exp ( ξ ∧ ) ⋅ δ ξ ∧ p δ ξ = lim δ ξ → 0 [ R t 0 1 ] [ δ ϕ ∧ δ ρ 0 1 ] p δ ξ = lim δ ξ → 0 [ R δ ϕ ∧ p + R δ ρ 0 ] δ ξ = lim δ ξ → 0 [ − ( R p ) ∧ ( R δ ϕ ) + R δ ρ 0 ] [ δ ρ , δ ϕ ] T = [ R − ( R p ) ∧ R 0 T 0 ] \begin{aligned}\frac{\partial(Tp)}{\partial\delta\xi}&=\lim_{\delta\xi\rightarrow0}\frac{\exp(\xi^\land )\exp(\delta\xi^\land)p - \exp(\xi^\land)p}{\varphi} \\\\&=\lim_{\delta\xi\rightarrow0}\frac{\exp(\xi^\land)(I + \delta\xi^\land)p - \exp(\xi^\land)p}{\delta\xi} \\\\&=\lim_{\delta\xi\rightarrow0}\frac{\exp(\xi^\land)·\delta\xi^\land p}{\delta\xi} \\\\&=\lim_{\delta\xi\rightarrow0}\frac{\begin{bmatrix}R&t\\\\0&1\end{bmatrix}\begin{bmatrix}\delta\phi^\land&\delta\rho\\\\0&1\end{bmatrix}p}{\delta\xi} \\\\&=\lim_{\delta\xi\rightarrow0}\frac{\begin{bmatrix}R\delta\phi^\land p+R\delta \rho\\\\0\end{bmatrix}}{\delta\xi} \\\\&=\lim_{\delta\xi\rightarrow0}\frac{\begin{bmatrix}-(Rp)^\land(R\delta\phi)+R\delta \rho\\\\0\end{bmatrix}}{[\delta\rho,\delta\phi]^T} \\\\&=\begin{bmatrix}R&-(Rp)^\land R\\\\0^T&0\end{bmatrix} \end{aligned} ∂δξ∂(Tp)=δξ→0limφexp(ξ∧)exp(δξ∧)p−exp(ξ∧)p=δξ→0limδξexp(ξ∧)(I+δξ∧)p−exp(ξ∧)p=δξ→0limδξexp(ξ∧)⋅δξ∧p=δξ→0limδξ⎣ ⎡R0t1⎦ ⎤⎣ ⎡δϕ∧0δρ1⎦ ⎤p=δξ→0limδξ⎣ ⎡Rδϕ∧p+Rδρ0⎦ ⎤=δξ→0lim[δρ,δϕ]T⎣ ⎡−(Rp)∧(Rδϕ)+Rδρ0⎦ ⎤=⎣ ⎡R0T−(Rp)∧R0⎦ ⎤