SLAM从入门到放弃：SLAM十四讲第三章习题

以下均为简单笔记，如有错误，请多多指教。

1. 验证旋转矩阵是正交矩阵。
   证：如公式(3.5)所示
   $R= \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{'} e_2^{'} e_3^{'} \end{bmatrix}$R=⎣⎡​e1T​e2T​e3T​​⎦⎤​[e1′​e2′​e3′​​]
   考虑到
   $\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^T e_2^T e_3^T \end{bmatrix}=I, \begin{bmatrix} e_1^{'} e_2^{'} e_3^{'} \end{bmatrix} \begin{bmatrix} e_1^{'T} e_2^{'T} e_3^{'T} \end{bmatrix}=I$⎣⎡​e1T​e2T​e3T​​⎦⎤​[e1T​e2T​e3T​​]=I,[e1′​e2′​e3′​​][e1′T​e2′T​e3′T​​]=I
   则不难发现$RR^T=R^TR=I$RRT=RTR=I。
   
2. 寻找罗德里格斯公式的推导过程并加以理解。
   证：
   
   如上图(图片来源 Computer Vision: Algorithm and Application)所示假定旋转轴为$\hat{n}$n^，$\theta$θ为旋转角度，$v,u$v,u为旋转前后的变量，即$u=Rv$u=Rv，证明
   $R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^{\land}（见公式3.14）$R=cosθI+(1−cosθ)nnT+sinθn∧（见公式3.14）
   首先不难发现，
   $v_{||}=\hat{n}(\hat{n}\bullet v)=\hat{n}\hat{n}^Tv$v∣∣​=n^(n^∙v)=n^n^Tv
   则
   $v_{\perp}=v-v_{||}=(I-\hat{n}\hat{n}^T)v$v⊥​=v−v∣∣​=(I−n^n^T)v
   进一步可以得到
   $v_{\times}=\hat{n} \times v = n^{\land} v$v×​=n^×v=n∧v
   于此同时，
   $u_{\perp} = cos\theta v_{\perp} + sin\theta v_{\times}$u⊥​=cosθv⊥​+sinθv×​
   又因为（原因是$u,v$u,v都是单位向量）
   $u_{||} = v_{||}$u∣∣​=v∣∣​
   整理上述所有公式可以得到：
   $u = u_{\perp} + u_{||} = cos\theta v_{\perp} + sin\theta v_{\times} + v_{||}$u=u⊥​+u∣∣​=cosθv⊥​+sinθv×​+v∣∣​
   整理后得到
   $u = cos\theta (I-\hat{n}\hat{n}^T)v + sin\theta n^{\land} v + \hat{n}\hat{n}^Tv = ( cos\theta I + (1-cos\theta)\hat{n}\hat{n}^T+sin\theta n^{\land})v$u=cosθ(I−n^n^T)v+sinθn∧v+n^n^Tv=(cosθI+(1−cosθ)n^n^T+sinθn∧)v
   得证。
   
3. 验证四元数旋转某个点后，结果是一个虚四元数，所有仍然对应到一个三维空间点。
   证：
   记点$p=[0,v]$p=[0,v]，四元数为$q=[cos\frac{\theta}{2},nsin\frac{\theta}{2}]$q=[cos2θ​,nsin2θ​]，则$q^{-1}=[cos\frac{\theta}{2},-nsin\frac{\theta}{2}]$q−1=[cos2θ​,−nsin2θ​]
   首先计算
   $qp=[cos\frac{\theta}{2}*0-sin\frac{\theta}{2}n^Tv,cos\frac{\theta}{2}v+sin\frac{\theta}{2}n_{\times}v]$qp=[cos2θ​∗0−sin2θ​nTv,cos2θ​v+sin2θ​n×​v]
   然后计算$(qp)q^{-1}$(qp)q−1的实部：
   $-sin\frac{\theta}{2}n^Tv*cos\frac{\theta}{2}-(cos\frac{\theta}{2}v+sin\frac{\theta}{2}n_{\times}v)^T*(-nsin\frac{\theta}{2})$−sin2θ​nTv∗cos2θ​−(cos2θ​v+sin2θ​n×​v)T∗(−nsin2θ​)
   由于$n_{\times}$n×​是反对称矩阵，即$n_{\times}^T=-n_{\times}$n×T​=−n×​；并且明显可知$n_{\times}n=0$n×​n=0
   $-sin\frac{\theta}{2}cos\frac{\theta}{2}n^Tv+sin\frac{\theta}{2}cos\frac{\theta}{2}v^Tn+sin\frac{\theta}{2}sin\frac{\theta}{2}v^Tn_{\times}n$−sin2θ​cos2θ​nTv+sin2θ​cos2θ​vTn+sin2θ​sin2θ​vTn×​n
   上式在此简化后可得到，由于$v^Tn$vTn和$v^Tn$vTn都是标量，因此$n^Tv=v^Tn$nTv=vTn
   $sin\frac{\theta}{2}cos\frac{\theta}{2}(v^Tn-v^Tn)=0$sin2θ​cos2θ​(vTn−vTn)=0
   得证。