1. 程式人生 > >SLAM從入門到放棄:SLAM十四講第三章習題

SLAM從入門到放棄:SLAM十四講第三章習題

以下均為簡單筆記,如有錯誤,請多多指教。

  1. 驗證旋轉矩陣是正交矩陣。 證:如公式(3.5)所示 R=[e1Te2Te3T][e1e2e3] R= \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{'} e_2^{'} e_3^{'} \end{bmatrix} 考慮到 [e1Te2Te3T][e1Te2Te3T]=I,[e1e2e3][e1Te2T
    e3T]=I \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
    則不難發現RRT=RTR=IRR^T=R^TR=I
  2. 尋找羅德里格斯公式的推導過程並加以理解。 證: 在這裡插入圖片描述 如上圖(圖片來源 Computer Vision: Algorithm and Application)所示假定旋轉軸為n^\hat{n}θ\theta為旋轉角度,v,uv,u為旋轉前後的變數,即u=Rvu=Rv,證明 R=cosθI+(1cosθ)nnT+sinθn3.14 R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^{\land}(見公式3.14)
    首先不難發現, v=n^(n^v)=n^n^Tv v_{||}=\hat{n}(\hat{n}\bullet v)=\hat{n}\hat{n}^Tv v=vv=(In^n^T)v v_{\perp}=v-v_{||}=(I-\hat{n}\hat{n}^T)v 進一步可以得到 v×=n^×v=nv v_{\times}=\hat{n} \times v = n^{\land} v 於此同時, u=cosθv+sinθv× u_{\perp} = cos\theta v_{\perp} + sin\theta v_{\times} 又因為(原因是u,vu,v都是單位向量) u=v u_{||} = v_{||} 整理上述所有公式可以得到: u=u+u=cosθv+sinθv×+v u = u_{\perp} + u_{||} = cos\theta v_{\perp} + sin\theta v_{\times} + v_{||} 整理後得到 u=cosθ(In^n^T)v+sinθnv+n^n^Tv=(cosθI+(1cosθ)n^n^T+sinθn)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 得證。
  3. 驗證四元數旋轉某個點後,結果是一個虛四元數,所有仍然對應到一個三維空間點。 證: 記點p=[0,v]p=[0,v],四元數為q=[cosθ2,nsinθ2]q=[cos\frac{\theta}{2},nsin\frac{\theta}{2}],則q1=[cosθ2,nsinθ2]q^{-1}=[cos\frac{\theta}{2},-nsin\frac{\theta}{2}] 首先計算 qp=[cosθ20sinθ2nTv,cosθ2v+sinθ2n×v] 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)q1(qp