《视觉SLAM十四讲》公式推导（一）

CH3 三维空间刚体运动

CH3-1 旋转矩阵的推导

（1）二维空间中的旋转矩阵

易得

$$\{\begin{array}{c}{x}^{\prime }=|O{P}^{\prime }|cos(\theta +\beta )=|OP|(cos\theta \cdot cos\beta -sin\theta \cdot sin\beta )=xcos\beta -ysin\beta \\ y^{\prime }=|O{P}^{\prime }|sin(\theta +\beta )=|OP|(sin\theta \cdot cos\beta +cos\theta \cdot sin\beta )=ycos\beta +xsin\beta \end{array}$$

写为矩阵形式即

$$\left[\begin{array}{l}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{lc}cos\beta & -sin\beta \\ sin\beta & cos\beta \end{array}\right]\left[\begin{array}{l}x\\ y\end{array}\right]$$

（2）三维空间中的旋转矩阵

以绕 $z$ 轴旋转为例：

将向量投影到 X-Y 平面，类似的有

$$\{\begin{array}{c}{x}^{\prime }=xcos\beta -ysin\beta \\ y^{\prime }=ycos\beta +xsin\beta \\ z^{\prime }=z\end{array}\text{\hspace{1em}(3-1-1)}$$

写成矩阵形式

$$\left[\begin{array}{l}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]=\left[\begin{array}{lcc}cos\beta & -sin\beta & 0\\ sin\beta & cos\beta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}x\\ y\\ z\end{array}\right]\text{\hspace{1em}(3-1-2)}$$

记为

$${\mathbf{a}}^{\prime }=\mathbf{Ra}\text{\hspace{1em}(3-1-3)}$$

将式中 $\beta$ 改为 $-\beta$，来表示向量 OP’ 顺时针旋转 $-\beta$ 角度后，回到 OP 的过程，此时

$$\left[\begin{array}{l}x\\ y\\ z\end{array}\right]=\left[\begin{array}{lcc}cos\beta & sin\beta & 0\\ -sin\beta & cos\beta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{l}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]\text{\hspace{1em}(3-1-4)}$$

可以看出

$$\mathbf{a}={\mathbf{R}}^{\mathbf{T}}{\mathbf{a}}^{\prime }\text{\hspace{1em}(3-1-5)}$$

由于 $R$ 是正交矩阵，也即

$$\mathbf{a}={\mathbf{R}}^{-1}{\mathbf{a}}^{\prime }\text{\hspace{1em}(3-1-6)}$$

也就是说， ${\mathbf{R}}^{-1}$ 表示向量经反方向旋转回到原向量的过程。

当然，也可以单纯从数学角度推导，将式（3-1-3） 两端分别左乘${\mathbf{R}}^{-1}$，同样可以得到式 (3-1-5)。

CH3-2 旋转矩阵是正交矩阵的证明

（1）只需证明 $\mathbf{R}{\mathbf{R}}^{\mathbf{T}}=\mathbf{I}$ 即证明 ${\mathbf{R}}^{\mathbf{T}}={\mathbf{R}}^{-1}$ 即可。

（2）分块矩阵转置：先对整体进行转置，再对每一个分块进行转置，例如向量$\mathbf{A}=[{\mathbf{A}}_1,{\mathbf{A}}_2,{\mathbf{A}}_3,...,{\mathbf{A}}_{\mathbf{N}}]$，则其转置为

$${\mathbf{A}}^T=\left[\begin{array}{l}{{\mathbf{A}}_1}^T\\ {{\mathbf{A}}_2}^T\\ {{\mathbf{A}}_3}^T\\ ...\\ {{\mathbf{A}}_{\mathbf{N}}}^T\end{array}\right]\text{\hspace{1em}(3-2-1)}$$

（3）$(\mathbf{AB}{)}^{\mathbf{T}}={\mathbf{B}}^{\mathbf{T}}{\mathbf{A}}^{\mathbf{T}}$

（4）下面证明旋转矩阵 $\mathbf{R}$ 是正交矩阵：

不妨设变换前后坐标系基底分别为 $[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3]$ 和 $[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }]$，由于向量本身并未改变，则有

$$\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[{\mathbf{e}}_1^{\prime },{\mathbf{e}}_2^{\prime },{\mathbf{e}}_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(3-2-2)}$$

将上式两端分别左乘 $\left[\begin{array}{l}{\mathbf{e}}_1^{\mathbf{T}}\\ {\mathbf{e}}_2^T\\ {\mathbf{e}}_3^T\end{array}\right]$ 得到，

$$\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{lll}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(3-2-3)}$$

则

$$\mathbf{R}=\left[\begin{array}{lll}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\text{\hspace{1em}(3-2-4)}$$

这是向量 ${\mathbf{a}}^{\prime }$ 经旋转得到 $\mathbf{a}$ 的旋转矩阵。

再将式 (3-8) 两端分别左乘 $\left[\begin{array}{l}{\mathbf{e}}_1^{\prime \mathbf{T}}\\ {\mathbf{e}}_2^{\prime T}\\ {\mathbf{e}}_3^{\prime T}\end{array}\right]$ 得到，

$$\left[\begin{array}{lll}{\mathbf{e}}_1^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_1^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_1^{\prime T}{\mathbf{e}}_3\\ {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_3\\ {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{l}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\text{\hspace{1em}(3-2-5)}$$

显然，这是向量 $\mathbf{a}$ 经过反方向旋转得到 ${\mathbf{a}}^{\prime }$ 的过程，那么有

$${\mathbf{R}}^{-1}=\left[\begin{array}{lll}{\mathbf{e}}_1^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_1^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_1^{\prime T}{\mathbf{e}}_3\\ {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_2^{\prime T}{\mathbf{e}}_3\\ {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_1& {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_2& {\mathbf{e}}_3^{\prime T}{\mathbf{e}}_3\end{array}\right]\text{\hspace{1em}(3-2-6)}$$

根据分块矩阵转置规则，易得

$${\mathbf{R}}^{\mathbf{T}}={\mathbf{R}}^{-1}\text{\hspace{1em}(3-2-7)}$$

证毕。

CH3-3 变换矩阵的逆的推导

（1）初等行变换法求矩阵的逆：将矩阵 $\mathbf{A}$ 写成增广矩阵的形式即 $(\mathbf{A}|\mathbf{I})$ ，经初等行变换，变为 $(\mathbf{I}|{\mathbf{A}}^{-1})$，则求出矩阵 $\mathbf{A}$ 的逆。

（2）已知三维空间变换矩阵为：

$$\mathbf{T}=\left[\begin{array}{ll}{\mathbf{R}}_{3\times 3}& {\mathbf{t}}_{3\times 1}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(3-3-1)}$$

写成增广矩阵

$$\left[\begin{array}{cccc}{\mathbf{R}}_{3\times 3}& {\mathbf{t}}_{3\times 1}& {\mathbf{E}}_{3\times 3}& 0\\ 0^{\mathrm{T}}& 1& 0& 1\end{array}\right]\text{\hspace{1em}(3-3-2)}$$

经过初等行变换，将前两列变为单位矩阵时，后两列即为 ${\mathbf{T}}^{-1}$。

首先，将第一行左乘 ${\mathbf{R}}_{3\times 3}^{-1}$，上式变为

$$\left[\begin{array}{cccc}{\mathbf{E}}_{3\times 3}& {\mathbf{R}}_{3\times 3}^{-1}{\mathbf{t}}_{3\times 1}& {\mathbf{R}}_{3\times 3}^{-1}& 0_{3\times 1}\\ 0^{\mathrm{T}}& 1& 0& 1\end{array}\right]\text{\hspace{1em}(3-3-3)}$$

再将第二行乘 $-{\mathbf{R}}_{3\times 3}^{-1}{\mathbf{t}}_{3\times 1}$ ，加到第一行上，得

$$\left[\begin{array}{cccc}{\mathbf{E}}_{3\times 3}& 0_{3\times 1}& {\mathbf{R}}_{3\times 3}^{-1}& -{\mathbf{R}}_{3\times 3}^{-1}{\mathbf{t}}_{3\times 1}\\ 0^{\mathrm{T}}& 1& 0& 1\end{array}\right]\text{\hspace{1em}(3-3-4)}$$

又因为 $\mathbf{R}$ 为正交矩阵，有 ${\mathbf{R}}^{-1}={\mathbf{R}}^{\mathbf{T}}$。

则变换矩阵的逆矩阵为

$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{T}}& -{\mathbf{R}}^{\mathrm{T}}\mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\text{\hspace{1em}(3-3-5)}$$

CH3-4 罗德里格斯公式推导

（1）注意绕轴旋转的方式，（类似直线绕轴旋转形成圆锥的过程），也就是说，该直线是母线，所绕的轴是中心线。

（2）向量点乘有交换律 $\vec{a}\cdot \vec{b}=\vec{b}\cdot \vec{a}$ ；没有结合律，即 $(\vec{a}\cdot \vec{b})\cdot \vec{c}\ne \vec{a}\cdot (\vec{b}\cdot \vec{c})$。矩阵乘法有结合律即 $(\mathbf{AB})\mathbf{C}=\mathbf{A}(\mathbf{BC})$。

（3）向量投影定理

如图，有

$$|\vec{a}|cos\theta =\frac{\vec{a}\cdot \vec{b}}{|\vec{b}|}=\vec{a}\cdot \overrightarrow{b_0}\text{\hspace{1em}(3-4-1)}$$

其中 $\overrightarrow{b_0}$为与向量 $\vec{b}$ 同向的单位向量。

式（3-19）表示向量 $\vec{a}$ 在向量 $\vec{b}$ 上的投影的模长为 $\vec{a}\cdot \overrightarrow{b_0}$，那么，其在 $\vec{b}$ 上的投影向量为 $(\vec{a}\cdot \overrightarrow{b_0})\cdot \overrightarrow{b_0}$。

（4）下面进行罗德里格斯公式的推导：

如上图所示，向量 $\vec{v}$ 绕 $\vec{u}$ 旋转 $\theta$ 得到 $\overrightarrow{v^{\prime }}$，其中 $\vec{u}$ 是单位向量 。

① 向量 $\vec{v}$分解： $\vec{v}={\vec{v}}_{\parallel }+{\vec{v}}_{\perp }$；

② 根据向量投影定理，${\vec{v}}_{\parallel }=(\vec{v}\cdot \vec{u})\cdot \vec{u}$

③ 综合 ① ② ，得

$${\vec{v}}_{\perp }=\vec{v}-{\vec{v}}_{\parallel }=\vec{v}-(\vec{v}\cdot \vec{u})\cdot \vec{u}\text{\hspace{1em}(3-4-2)}$$

④ 借助辅助向量 $\vec{w}$，且 $\vec{w}=\vec{u}\times {\vec{v}}_{\perp }$，即向量$\vec{w}$、$\vec{u}$、${\vec{v}}_{\perp }$ 两两垂直。俯视图如下：

则有

$$\begin{array}{rl}\vec{w}=\vec{u}\times {\vec{v}}_{\perp }=& \vec{u}\times (\vec{v}-{\vec{v}}_{\parallel })\\ =& \vec{u}\times \vec{v}-\vec{u}\times {\vec{v}}_{\parallel }\\ =& \vec{u}\times \vec{v}\end{array}\text{\hspace{1em}(3-4-3)}$$

⑤ 将 ${\overrightarrow{v^{\prime }}}_{\perp }$ 投影到 ${\vec{v}}_{\perp }$ 和 $\vec{w} $ 上，则有

$${\overrightarrow{v^{\prime }}}_{\perp }={\vec{v}}_v^{\prime }+{\vec{v}}_w^{\prime }\text{\hspace{1em}(3-4-4)}$$

⑥ 由上图

$$|{\overrightarrow{v^{\prime }}}_{\perp }|=|\vec{w}|=|{\vec{v}}_{\perp }|\text{\hspace{1em}(3-4-5)}$$

且

$$|{\vec{v}}_w^{\prime }|=|{\overrightarrow{v^{\prime }}}_{\perp }|sin\theta =|\vec{w}|sin\theta$$

结合式（2-23），有

$${\vec{v}}_w^{\prime }=\vec{w}sin\theta \text{\hspace{1em}(3-4-6)}$$

同理

$$|{\vec{v}}_v^{\prime }|=|{\vec{v}}_{\perp }|cos\theta$$

得

$${\vec{v}}_v^{\prime }={\vec{v}}_{\perp }cos\theta \text{\hspace{1em}(3-4-7)}$$

⑦ 综合式（3-22）、（3-24）、（3-25），得

$${\overrightarrow{v^{\prime }}}_{\perp }={\vec{v}}_v^{\prime }+{\vec{v}}_w^{\prime }={\vec{v}}_{\perp }cos\theta +\vec{w}sin\theta \text{\hspace{1em}(3-4-8)}$$

⑧ 综上，

$$\begin{array}{rl}\overrightarrow{v^{\prime }}=& {\vec{v}}_{\parallel }+{\overrightarrow{v^{\prime }}}_{\perp }\\ =& (\vec{v}\cdot \vec{u})\cdot \vec{u}+{\vec{v}}_{\perp }cos\theta +\vec{w}sin\theta \\ =& (\vec{v}\cdot \vec{u})\cdot \vec{u}+(\vec{v}-(\vec{v}\cdot \vec{u})\cdot \vec{u})cos\theta +\vec{u}\times \vec{v}sin\theta \\ =& (\vec{v}\cdot \vec{u})\cdot \vec{u}+sin\theta \vec{u}\times \vec{v}+cos\theta \vec{v}-cos\theta (\vec{v}\cdot \vec{u})\cdot \vec{u}\\ =& cos\theta \vec{v}+(1-cos\theta )(\vec{v}\cdot \vec{u})\cdot \vec{u}+sin\theta \vec{u}\times \vec{v}\end{array}\text{\hspace{1em}(3-4-9)}$$

⑨ 设矩阵

$$\mathbf{u}=\left[\begin{array}{c}{u}_x\\ u_y\\ u_z\end{array}\right]，\mathbf{v}=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$$

1）将向量点乘转换为矩阵乘法，（此处应用向量交换律和矩阵结合律）

$$(\vec{v}\cdot \vec{u})\cdot \vec{u}=\vec{u}\cdot (\vec{v}\cdot \vec{u})=\mathbf{u}({\mathbf{u}}^T\mathbf{v})=\mathbf{u}{\mathbf{u}}^T\mathbf{v}\text{\hspace{1em}(3-4-10)}$$

2）写成反对称矩阵形式

$$\vec{u}\times \vec{v}=\left|\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ u_x& u_y& u_z\\ v_x& v_y& v_z\end{array}\right|=\left[\begin{array}{c}{u}_y{v}_z-u_z{v}_y\\ u_z{v}_x-u_x{v}_z\\ u_x{v}_y-u_y{v}_x\end{array}\right]=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right]\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]={\mathbf{u}}^{\wedge }\mathbf{v}\text{\hspace{1em}(3-4-11)}$$

⑩ 因为 $ \boldsymbol{v’}=\boldsymbol{R}\boldsymbol{v}$

综上所述，式（3-27）可化为

$$\mathbf{R}=cos\theta \mathbf{I}+(1-cos\theta )\mathbf{u}{\mathbf{u}}^T+sin\theta {\mathbf{u}}^{\wedge }\text{\hspace{1em}(3-4-12)}$$

证毕。

（5）旋转矩阵到旋转向量的转换

将式（3-4-12）两边取迹，得

$$\begin{array}{rl}tr(\mathbf{R})=& cos\theta tr(\mathbf{I})+(1-cos\theta )tr(\mathbf{u}{\mathbf{u}}^T)+sin\theta tr({\mathbf{u}}^{\wedge })\\ =& 3cos\theta +(1-cos\theta )+0\\ =& 1+2cos\theta \end{array}\text{\hspace{1em}(3-4-13)}$$

其中，$tr(\mathbf{u}{\mathbf{u}}^T)=u_x^2+u_y^2+u_z^2=1$（向量$\mathbf{u}$是单位向量，模长为1）。

那么，对于转角 $\theta$ 有

$$\theta =arccos(\frac{tr(\mathbf{R})-1}{2})\text{\hspace{1em}(3-4-14)}$$

对于转轴 $\mathbf{u}$ 有，

$$\mathbf{Ru}=\mathbf{u}\text{\hspace{1em}(3-4-15)}$$

即转轴 $\mathbf{u}$ 是旋转矩阵 $\mathbf{R}$ 特征值为 1 对应的特征向量。