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

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/u012348774/article/details/83588897

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

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}$
   考虑到
   $\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$
   则不难发现 $RR^T=R^TR=I$。
   
2. 寻找罗德里格斯公式的推导过程并加以理解。
   证：
   
   如上图(图片来源 Computer Vision: Algorithm and Application)所示假定旋转轴为 $\hat{n}$， $\theta$为旋转角度， $v,u$为旋转前后的变量，即 $u=Rv$，证明
   $R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^{\land}（见公式3.14）$
   首先不难发现，
   $v_{||}=\hat{n}(\hat{n}\bullet v)=\hat{n}\hat{n}^Tv$
   则
   $v_{\perp}=v-v_{||}=(I-\hat{n}\hat{n}^T)v$
   进一步可以得到
   $v_{\times}=\hat{n} \times v = n^{\land} v$
   于此同时，
   $u_{\perp} = cos\theta v_{\perp} + sin\theta v_{\times}$
   又因为（原因是 $u,v$都是单位向量）
   $u_{||} = v_{||}$
   整理上述所有公式可以得到：
   $u = u_{\perp} + u_{||} = cos\theta v_{\perp} + sin\theta v_{\times} + 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]$，四元数为 $q=[cos\frac{\theta}{2},nsin\frac{\theta}{2}]$，则 $q^{-1}=[cos\frac{\theta}{2},-nsin\frac{\theta}{2}]$
   首先计算
   $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)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})$
   由于 $n_{\times}$是反对称矩阵，即 $n_{\times}^T=-n_{\times}$；并且明显可知 $n_{\times}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$
   上式在此简化后可得到，由于 $v^Tn$和 $v^Tn$都是标量，因此 $n^Tv=v^Tn$
   $sin\frac{\theta}{2}cos\frac{\theta}{2}(v^Tn-v^Tn)=0$
   得证。