视觉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\ast R_2$仍然是行列式为1的正交阵）
幺元：$I\cdot R=R\cdot I=R$
逆：$\det (R)=1\Rightarrow R$可逆 同时$R^{-1}=R^T$

          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=⎣ ⎡​R0T​t1​⎦ ⎤​∈R4×4∣R∈SO(3),t∈R3}

封闭性：两次欧式变换连乘等于一个新的欧式变换（代数角度验证也可，$T_1\ast T_2$仍满足上述形式）
幺元：$I\cdot T=T\cdot I=T$，表示不进行旋转和平移
逆：$T=\left[\begin{array}{cc}R& t\\ & \\ 0^T& 1\end{array}\right]$根据分块矩阵求逆公式${\left[\begin{array}{cc}A& C\\ & \\ 0& B\end{array}\right]}^{-1}=\left[\begin{array}{cc}{A}^{-1}& -A^{-1}C{B}^{-1}\\ & \\ 0& B^{-1}\end{array}\right]$
$T^{-1}=\left[\begin{array}{cc}{R}^{-1}& -R^{-1}t\\ & \\ 0^T& 1\end{array}\right]\in SE(3)$

         $Sim(3)=\left\{\begin{array}{c}S=\left[\begin{array}{cc}sR& t\\ & \\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\end{array}\right\}$

封闭性：两次相似变换连乘等于一个新的相似变换（代数角度验证也可，$S_1\ast S_2$仍满足上述形式）
幺元：单位矩阵，表示不进行旋转、平移、缩放
逆：$a^{{}^{\prime }}=sRa+t$逆变换为$a=(R^T{a}^{{}^{\prime }}-R^{T}t)/s$

2. 验证$({\mathbb{R}}^3,\mathbb{R},\times )$构成李代数

封闭性：集合中任意两个元素通过李括号运算得到的结果仍属于集合，三维向量叉乘三维向量，得到的结果仍然是三维向量
双线性：乘法分配律
自反性：$R\times R=|R||R|\sin \theta =0(\theta =0^{°})$
雅可比等价：可以通过向量的拉格朗日公式：

         $a\times (b\times c)=b(a\cdot c)-c(a\cdot b)$
  代入下式：
         $a\times (b\times c)+c\times (a\times b)+b\times (c\times a)$
  可得结果为0

3. 验证$\mathfrak{so}(3)$和$\mathfrak{se}(3)$满足李代数要求的性质

李代数$\mathfrak{so}(3)$的集合为3维向量$\varphi$。其对应反对称矩阵$\Phi$,两个向量${\varphi }_1,{\varphi }_2$的李括号为$\left[\begin{array}{c}{\varphi }_1,{\varphi }_2\end{array}\right]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$

封闭性：${\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1=-({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^T$仍为反对称矩阵，可对应$\varphi$

双线性：$[a{\varphi }_1+b{\varphi }_2,{\varphi }_3]=a[{\varphi }_1,{\varphi }_3]+b[{\varphi }_2,{\varphi }_3]$
即证
         $\begin{array}{rl}[(a{\Phi }_1+b{\Phi }_2)\cdot {\Phi }_3-{\Phi }_3\cdot (a{\Phi }_1+b{\Phi }_2){]}^{\vee }& =a({\Phi }_1{\Phi }_3-{\Phi }_3{\Phi }_1{)}^{\vee }+b({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2{)}^{\vee }\\ & =(a{\Phi }_1{\Phi }_3-a{\Phi }_3{\Phi }_1+b{\Phi }_2{\Phi }_3-b{\Phi }_3{\Phi }_2{)}^{\vee }\end{array}$

自反性：$[{\varphi }_1,{\varphi }_1]=({\Phi }_1{\Phi }_1-{\Phi }_1{\Phi }_1{)}^{\vee }=0$

雅可比等价：$[{\varphi }_1,[{\varphi }_2,{\varphi }_3]]+[{\varphi }_3,[{\varphi }_1,{\varphi }_2]]+[{\varphi }_2,[{\varphi }_3,{\varphi }_1]]=0$

         $\begin{array}{rl}[{\Phi }_1,({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2{)}^{\vee }]& =[{\Phi }_1({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2)-({\Phi }_2{\Phi }_3-{\Phi }_3{\Phi }_2){\Phi }_1{]}^{\vee }\\ & \\ & =({\Phi }_1{\Phi }_2{\Phi }_3-{\Phi }_1{\Phi }_3{\Phi }_2-{\Phi }_2{\Phi }_3{\Phi }_1+{\Phi }_3{\Phi }_2{\Phi }_1{)}^{\vee }=A^{\vee }\end{array}$

         $[{\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{)}^{\vee }=B^{\vee }$

         $[{\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{)}^{\vee }=C^{\vee }$

         $(A^{\vee }+B^{\vee }+C^{\vee })=(A+B+C{)}^{\vee }=0$

$\mathfrak{se}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{so}(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ & \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}.$
$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$
封闭性：即证$(({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }{)}^{\wedge }$仍满足$\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ & \\ 0^T& 0\end{array}\right]$的形式
         $\begin{array}{rl}\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ & \\ 0^T& 0\end{array}\right]-\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ & \\ 0^T& 0\end{array}\right]& =\left[\begin{array}{cc}{\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1& {\Phi }_1{\rho }_2-{\Phi }_2{\rho }_1\\ & \\ 0^T& 0\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}[{\varphi }_1,{\varphi }_2]& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]\end{array}$
         即$[{\xi }_1,{\xi }_2]=\left[\begin{array}{c}{\Phi }_1{\rho }_2-{\Phi }_2{\rho }_1\\ ,\\ [{\varphi }_1,{\varphi }_2]\end{array}\right]$

双线性：$[a{\xi }_1+b{\xi }_2,{\xi }_3]=a[{\xi }_1,{\xi }_3]+b[{\xi }_2,{\xi }_3]$

$\begin{array}{rl}& \left[\begin{array}{cc}a{\Phi }_1+b{\Phi }_2& a{\rho }_1+b{\rho }_2\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]-\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}a{\Phi }_1+b{\Phi }_2& a{\rho }_1+b{\rho }_2\\ & \\ 0^T& 0\end{array}\right]\\ & \\ & =a\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]-a\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_1& {\rho }_1\\ & \\ 0^T& 0\end{array}\right]+b\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]-b\left[\begin{array}{cc}{\Phi }_3& {\rho }_3\\ & \\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\Phi }_2& {\rho }_2\\ & \\ 0^T& 0\end{array}\right]\\ & \\ & =a[{\xi }_1,{\xi }_3]+b[{\xi }_2,{\xi }_3]\end{array}$

自反性：$[{\xi }_1,{\xi }_1]=({\xi }_1^{\wedge }{\xi }_1^{\wedge }-{\xi }_1^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }=0^{\vee }$

雅可比等价：
$\begin{array}{rl}[{\xi }_1,[{\xi }_2,{\xi }_3]]& =\left[\begin{array}{c}{\xi }_1,\left[\begin{array}{c}{\Phi }_2{\rho }_3-{\Phi }_3{\rho }_2\\ \\ ({\varphi }_2,{\varphi }_3)\end{array}\right]\end{array}\right]\\ & \\ & =\left[\begin{array}{c}{\Phi }_1({\Phi }_2{\rho }_3-{\Phi }_3{\rho }_2)-[{\varphi }_2,{\varphi }_3]{\rho }_1\\ \\ [{\varphi }_1,[{\varphi }_2,{\varphi }_3]]\end{array}\right]\\ & \\ & =\left[\begin{array}{c}{\Phi }_1{\Phi }_2{\rho }_3-{\Phi }_1{\Phi }_3{\rho }_2-{\Phi }_2{\Phi }_3{\rho }_1+{\Phi }_3{\Phi }_2{\rho }_1\\ \\ [{\varphi }_1,[{\varphi }_2,{\varphi }_3]]\end{array}\right]\end{array}$

因此最后三项相加可互相抵消（上半部分代数和为0，下半部分有$\mathfrak{so}(3)$雅可比等价可知为0）

4. 验证性质（4.20）和（4.21）

(4.20)
         $\begin{array}{rl}{a}^{\wedge }{a}^{\wedge }& =\left[\begin{array}{ccc}0& -a_3& a_2\\ & & \\ a_3& 0& -a_1\\ & & \\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -a_3& a_2\\ & & \\ a_3& 0& -a_1\\ & & \\ -a_2& a_1& 0\end{array}\right]\\ & \\ & =\left[\begin{array}{ccc}-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\end{array}\right]\\ & \\ & =\left[\begin{array}{ccc}{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\end{array}\right]-\left[\begin{array}{ccc}{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{array}\right]\\ & \\ & =a{a}^T-I\end{array}$

(4.21)
         $\begin{array}{r}{a}^{\wedge }a{a}^T=\left[\begin{array}{ccc}0& -a_3& a_2\\ & & \\ a_3& 0& -a_1\\ & & \\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{c}{a}_1\\ \\ a_2\\ \\ a_3\end{array}\right]=0\end{array}$
         $\begin{array}{rl}{a}^{\wedge }{a}^{\wedge }{a}^{\wedge }& =a^{\wedge }(a{a}^T-I)\\ & \\ & =a^{\wedge }a{a}^T-a^{\wedge }\\ & \\ & =-a^{\wedge }\end{array}$

5. 证明：
         $\begin{array}{rl}& R{p}^{\wedge }{R}^T=(Rp{)}^{\wedge }\\ & \\ & \iff R{p}^{\wedge }=(Rp{)}^{\wedge }R.\\ & \\ & \iff \forall u\in R^{3}R{p}^{\wedge }u=(Rp{)}^{\wedge }Ru\\ & \\ & \iff \forall u\in R^{3}R(p\times u)=(Rp)\times (Ru)\end{array}$

                  最后一式利用向量叉乘的旋转变换不变性

  从三维几何的角度来理解：$v,u$ 是任意两个三维向量，$(v\times u)$是一个和 $v,u$ 都垂直、大小为 $\mid v\mid \mid u\mid sin(u,v)$ 的三维向量；将 $v,u,v\times u$ 三个向量都经过同一个旋转，它们的相对位姿和模长都不会改变，所以 $(Rv)$ 和 $(Ru)$ 的叉乘仍是 $R(v\times u)$。

6. 证明：
         $R\exp (p^{\wedge })R^T=\exp ((R{p}^{\wedge })).$

  该式称为$SO(3)$上的伴随性质。同样地，在$SE(3)$上也有伴随性质：

         $T\exp ({\xi }^{\wedge })T^{-1}=\exp ((Ad(T)\xi {)}^{\wedge }),$

  其中：

         $Ad(T)=\left[\begin{array}{cc}R& t^{\wedge }R\\ & \\ 0& R\end{array}\right].$

         $\begin{array}{rl}\exp ((Rp{)}^{\wedge })& =\exp (R{p}^{\wedge }{R}^T)\\ & \\ & =\sum _{n=0}^{\infty }\frac{1}{n!}(R{p}^{\wedge }{R}^T{)}^n\\ & \\ & =\sum _{n=0}^{\infty }\frac{1}{n!}(R{p}^{\wedge }{R}^T)\cdot (R{p}^{\wedge }{R}^T)\cdot \cdot \cdot (R{p}^{\wedge }{R}^T)\\ & \\ & (R^{T}R=I)\\ & \\ & =\sum _{n=0}^{\infty }\frac{1}{n!}R(p^{\wedge }{)}^n{R}^T\\ & \\ & =R\cdot (\sum _{n=0}^{\infty }\frac{1}{n!}(p^{\wedge }{)}^n)R^T\\ & \\ & =R\cdot \exp (p^{\wedge })R^T\end{array}$

7.依照左扰动的推导，推导SO(3)和SE(3)在右扰动下的导数。

SO(3)右扰动下的导数
设右扰动$\Delta R$对应的李代数为$\varphi$,对$\varphi$求导

         $\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })\exp ({\phi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })(I+{\phi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({\varphi }^{\wedge })\cdot {\phi }^{\wedge }p}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{R{\phi }^{\wedge }p}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{(R\phi {)}^{\wedge }\cdot (Rp)}{\phi }\\ & \\ & =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\cdot (R\phi )}{\phi }\\ & \\ & =-(Rp{)}^{{}^{\prime }}\cdot R\end{array}$

SE(3)右扰动下的导数,设右扰动$\Delta T$对应的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$

         $\begin{array}{rl}\frac{\partial (Tp)}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{\lim }\frac{\exp ({\xi }^{\wedge })\exp (\delta {\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\phi }\\ & \\ & =\underset{\delta \xi \to 0}{\lim }\frac{\exp ({\xi }^{\wedge })(I+\delta {\xi }^{\wedge })p-\exp ({\xi }^{\wedge })p}{\delta \xi }\\ & \\ & =\underset{\delta \xi \to 0}{\lim }\frac{\exp ({\xi }^{\wedge })\cdot \delta {\xi }^{\wedge }p}{\delta \xi }\\ & \\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}R& t\\ & \\ 0& 1\end{array}\right]\left[\begin{array}{cc}\delta {\varphi }^{\wedge }& \delta \rho \\ & \\ 0& 1\end{array}\right]p}{\delta \xi }\\ & \\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}R\delta {\varphi }^{\wedge }p+R\delta \rho \\ \\ 0\end{array}\right]}{\delta \xi }\\ & \\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{c}-(Rp{)}^{\wedge }(R\delta \varphi )+R\delta \rho \\ \\ 0\end{array}\right]}{[\delta \rho ,\delta \varphi {]}^T}\\ & \\ & =\left[\begin{array}{cc}R& -(Rp{)}^{\wedge }R\\ & \\ 0^T& 0\end{array}\right]\end{array}$