《视觉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)}$$

写成矩阵形式

[ x ′ y ′ z ′ ] = [ c o s β − s i n β 0 s i n β c o s β 0 0 0 1 ] [ x y z ] (3-1-2) \left[\begin{array}{l} x' \\ y' \\ z' \end{array}\right]= \left[\begin{array}{l} 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] \tag{3-1-2} ​x′y′z′​ ​= ​cosβsinβ0​−sinβcosβ0​001​ ​ ​xyz​ ​(3-1-2)

记为

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

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

[ x y z ] = [ c o s β s i n β 0 − s i n β c o s β 0 0 0 1 ] [ x ′ y ′ z ′ ] (3-1-4) \left[\begin{array}{l} x \\ y \\ z \end{array}\right]= \left[\begin{array}{l} 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] \tag{3-1-4} ​xyz​ ​= ​cosβ−sinβ0​sinβcosβ0​001​ ​ ​x′y′z′​ ​(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}}]$，则其转置为

A T = [ A 1 T A 2 T A 3 T . . . A N T ] (3-2-1) \boldsymbol{A}^T=\left[\begin{array}{l} \boldsymbol{A_1}^T\\ \boldsymbol{A_2}^T \\ \boldsymbol{A_3}^T \\ ...\\ \boldsymbol{A_N}^T \end{array}\right] \tag{3-2-1} AT= ​A1​TA2​TA3​T...AN​T​ ​(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 }]$，由于向量本身并未改变，则有

[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (3-2-2) \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \tag{3-2-2} [e1​,e2​,e3​] ​a1​a2​a3​​ ​=[e1′​,e2′​,e3′​] ​a1′​a2′​a3′​​ ​(3-2-2)

将上式两端分别左乘 [ e 1 T e 2 T e 3 T ] \left[\begin{array}{l} \boldsymbol{e_{1}^T} \\ \boldsymbol{e}_{2}^T \\ \boldsymbol{e}_{3}^T \end{array}\right] ​e1T​e2T​e3T​​ ​ 得到，

[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (3-2-3) \left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{lll} \boldsymbol{e}_{1}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^T \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \tag{3-2-3} ​a1​a2​a3​​ ​= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​ ​a1′​a2′​a3′​​ ​(3-2-3)

则

R = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] (3-2-4) \boldsymbol{R}=\left[\begin{array}{lll} \boldsymbol{e}_{1}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^T \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^T \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^T \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^T \boldsymbol{e}_{3}^{\prime} \end{array}\right] \tag{3-2-4} R= ​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​ ​(3-2-4)

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

再将式 (3-8) 两端分别左乘 [ e 1 ′ T e 2 ′ T e 3 ′ T ] \left[\begin{array}{l} \boldsymbol{e_{1}'^T} \\ \boldsymbol{e}_{2}'^T \\ \boldsymbol{e}_{3}'^T \end{array}\right] ​e1′T​e2′T​e3′T​​ ​ 得到，

[ e 1 ′ T e 1 e 1 ′ T e 2 e 1 ′ T e 3 e 2 ′ T e 1 e 2 ′ T e 2 e 2 ′ T e 3 e 3 ′ T e 1 e 3 ′ T e 2 e 3 ′ T e 3 ] [ a 1 a 2 a 3 ] = [ a 1 ′ a 2 ′ a 3 ′ ] (3-2-5) \left[\begin{array}{lll} \boldsymbol{e}_{1}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{1}'^T \boldsymbol{e}_{2} & \boldsymbol{e}_{1}'^T \boldsymbol{e}_{3}\\ \boldsymbol{e}_{2}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{2}'^T \boldsymbol{e}_{2}& \boldsymbol{e}_{2}'^T \boldsymbol{e}_{3} \\ \boldsymbol{e}_{3}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{3}'^T \boldsymbol{e}_{2} & \boldsymbol{e}_{3}'^T \boldsymbol{e}_{3} \end{array}\right]\left[\begin{array}{c} a_{1} \\ a_{2}\\ a_{3} \end{array}\right]= \left[\begin{array}{l} a_{1}' \\ a_{2}' \\ a_{3}' \end{array}\right] \tag{3-2-5} ​e1′T​e1​e2′T​e1​e3′T​e1​​e1′T​e2​e2′T​e2​e3′T​e2​​e1′T​e3​e2′T​e3​e3′T​e3​​ ​ ​a1​a2​a3​​ ​= ​a1′​a2′​a3′​​ ​(3-2-5)

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

R − 1 = [ e 1 ′ T e 1 e 1 ′ T e 2 e 1 ′ T e 3 e 2 ′ T e 1 e 2 ′ T e 2 e 2 ′ T e 3 e 3 ′ T e 1 e 3 ′ T e 2 e 3 ′ T e 3 ] (3-2-6) \boldsymbol{R^{-1}}=\left[\begin{array}{lll} \boldsymbol{e}_{1}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{1}'^T \boldsymbol{e}_{2} & \boldsymbol{e}_{1}'^T \boldsymbol{e}_{3}\\ \boldsymbol{e}_{2}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{2}'^T \boldsymbol{e}_{2}& \boldsymbol{e}_{2}'^T \boldsymbol{e}_{3} \\ \boldsymbol{e}_{3}'^T \boldsymbol{e}_{1} & \boldsymbol{e}_{3}'^T \boldsymbol{e}_{2} & \boldsymbol{e}_{3}'^T \boldsymbol{e}_{3} \end{array}\right] \tag{3-2-6} R−1= ​e1′T​e1​e2′T​e1​e3′T​e1​​e1′T​e2​e2′T​e2​e3′T​e2​​e1′T​e3​e2′T​e3​e3′T​e3​​ ​(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）向量点乘有交换律 $a\cdot b=b\cdot a$ ；没有结合律，即 $(a\cdot b)\cdot c\ne a\cdot (b\cdot c)$。矩阵乘法有结合律即 $(\mathbf{AB})\mathbf{C}=\mathbf{A}(\mathbf{BC})$。

（3）向量投影定理

如图，有

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

其中 $b_0$为与向量 $b$ 同向的单位向量。

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

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

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

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

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

③ 综合 ① ② ，得

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

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

则有

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

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

$${v^{\prime }}_{\perp }=v_v^{\prime }+v_w^{\prime }\text{\hspace{1em}(3-4-4)}$$

⑥ 由上图

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

且

$$|v_w^{\prime }|=|{v^{\prime }}_{\perp }|sin\theta =|w|sin\theta$$

结合式（2-23），有

$$v_w^{\prime }=wsin\theta \text{\hspace{1em}(3-4-6)}$$

同理

$$|v_v^{\prime }|=|v_{\perp }|cos\theta$$

得

$$v_v^{\prime }=v_{\perp }cos\theta \text{\hspace{1em}(3-4-7)}$$

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

$${v^{\prime }}_{\perp }=v_v^{\prime }+v_w^{\prime }=v_{\perp }cos\theta +wsin\theta \text{\hspace{1em}(3-4-8)}$$

⑧ 综上，

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

⑨ 设矩阵

u = [ u x u y u z ] ， v = [ v x v y v z ] \boldsymbol{u}=\left[\begin{array}{c} u_{x} \\ u_{y}\\ u_{z} \end{array}\right]， \boldsymbol{v}=\left[\begin{array}{c} v_{x} \\ v_{y}\\ v_{z} \end{array}\right] u= ​ux​uy​uz​​ ​，v= ​vx​vy​vz​​ ​

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

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

2）写成反对称矩阵形式

u ⃗ × v ⃗ = ∣ i j k u x u y u z v x v y v z ∣ = [ u y v z − u z v y u z v x − u x v z u x v y − u y v x ] = [ 0 − u z u y u z 0 − u x − u y u x 0 ] [ v x v y v z ] = u ∧ v (3-4-11) \vec{u}\times\vec{v}=\left| \begin{array}{cccc} \boldsymbol{i} & \boldsymbol{j} & \boldsymbol{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}{c} 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]=\boldsymbol{u}^{\wedge}\boldsymbol{v} \tag{3-4-11} u ×v = ​iux​vx​​juy​vy​​kuz​vz​​ ​= ​uy​vz​−uz​vy​uz​vx​−ux​vz​ux​vy​−uy​vx​​ ​= ​0uz​−uy​​−uz​0ux​​uy​−ux​0​ ​ ​vx​vy​vz​​ ​=u∧v(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 对应的特征向量。