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

CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果

前面已经推导过

$$v^{\prime }=pv{p}^{\ast }=pv{p}^{-1}$$

其中，$v=[0,\vec{v}]$，$p=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\vec{u}]$，代入上式

$$\begin{array}{rl}{v}^{\prime }& =pv{p}^{\ast }\\ & =[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\vec{u}][0,\vec{v}][\cos \frac{\theta }{2},-\sin \frac{\theta }{2}\vec{u}]\\ & =[0-\sin \frac{\theta }{2}\vec{u}\cdot \vec{v},\cos \frac{\theta }{2}\vec{v}+0+\sin \frac{\theta }{2}\vec{u}\times \vec{v}][\cos \frac{\theta }{2},-\sin \frac{\theta }{2}\vec{u}]\\ & =[-\sin \frac{\theta }{2}\vec{u}\cdot \vec{v},\cos \frac{\theta }{2}\vec{v}+\sin \frac{\theta }{2}\vec{u}\times \vec{v}][\cos \frac{\theta }{2},-\sin \frac{\theta }{2}\vec{u}]\end{array}\text{\hspace{1em}(3-8-1)}$$

分别计算实部和虚部

$$\begin{array}{rl}\mathrm{Re}& =-\sin \frac{\theta }{2}\cos \frac{\theta }{2}\vec{u}\cdot \vec{v}+(\cos \frac{\theta }{2}\vec{v}+\sin \frac{\theta }{2}\vec{u}\times \vec{v})\cdot \sin \frac{\theta }{2}\vec{u}\\ & =-\sin \frac{\theta }{2}\cos \frac{\theta }{2}\vec{u}\cdot \vec{v}+\cos \frac{\theta }{2}\sin \frac{\theta }{2}\vec{u}\cdot \vec{v}+\sin \frac{\theta }{2}(\vec{u}\times \vec{v})\cdot \vec{u}\\ & =0+0\\ & =0\end{array}\text{\hspace{1em}(3-8-2)}$$

$$\begin{array}{rl}\mathrm{Im}& =(-\sin \frac{\theta }{2}\vec{u}\cdot \vec{v})\cdot (-\sin \frac{\theta }{2}\vec{u})+(\cos \frac{\theta }{2}\vec{v}+\sin \frac{\theta }{2}\vec{u}\times \vec{v})\cos \frac{\theta }{2}\\ & +(\cos \frac{\theta }{2}\vec{v}+\sin \frac{\theta }{2}\vec{u}\times \vec{v})\times (-\sin \frac{\theta }{2}\vec{u})\end{array}\text{\hspace{1em}(3-8-3)}$$

我们希望将其写成矩阵乘法形式。

先证明公式：$\vec{a}\times (\vec{b}\times \vec{c})=(\vec{a}\cdot \vec{c})\cdot \vec{b}-(\vec{a}\cdot \vec{b})\cdot \vec{c}$。

证明：

$$\begin{array}{rl}\vec{a}\times \vec{b}& ={\mathbf{a}}^{\wedge }\mathbf{b}\\ & =\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{c}{b}_1\\ b_2\\ b_3\end{array}\right]\stackrel{△}{=}{\mathbf{T}}_a\mathbf{b}\end{array}\text{\hspace{1em}(3-8-4)}$$

那么（矩阵乘法满足结合律）

$$\vec{a}\times (\vec{b}\times \vec{c})=({\mathbf{T}}_a{\mathbf{T}}_b)\mathbf{c}={\mathbf{T}}_a{\mathbf{T}}_b\mathbf{c}$$

而

$$\begin{array}{rl}{\mathbf{T}}_a{\mathbf{T}}_b& =\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -b_3& b_2\\ b_3& 0& -b_1\\ -b_2& b_1& 0\end{array}\right]\\ & =\left[\begin{array}{ccc}-a_3{b}_3-a_2{b}_2& a_2{b}_1& a_3{b}_1\\ a_1{b}_2& -a_3{b}_3-a_1{b}_1& a_3{b}_2\\ a_1{b}_3& a_2{b}_3& -a_2{b}_2-a_1{b}_1\end{array}\right]\\ & =-(\vec{a}\cdot \vec{b})\mathbf{I}+\mathbf{b}{\mathbf{a}}^{\mathrm{T}}\end{array}$$

则（用到了矩阵结合律）

$$\begin{array}{rl}\vec{a}\times (\vec{b}\times \vec{c})={\mathbf{T}}_a{\mathbf{T}}_b\mathbf{c}& =(-(\vec{a}\cdot \vec{b})\mathbf{I}+\mathbf{b}{\mathbf{a}}^{\mathrm{T}})\mathbf{c}\\ & =-(\vec{a}\cdot \vec{b})\mathbf{c}+\mathbf{b}({\mathbf{a}}^{\mathrm{T}}\mathbf{c})\\ & =-(\vec{a}\cdot \vec{b})\mathbf{c}+(\vec{a}\cdot \vec{c})\mathbf{b}\end{array}\text{\hspace{1em}(3-8-5)}$$

同理可证

$$(\vec{a}\times \vec{b})\times \vec{c}=(\vec{a}\cdot \vec{c})\mathbf{b}-(\vec{b}\cdot \vec{c})\mathbf{a}\text{\hspace{1em}(3-8-6)}$$

证毕。

下面继续推导式（3-8-3）

$$\begin{array}{rl}\mathrm{Im}& ={\sin }^2\frac{\theta }{2}\vec{u}\cdot \vec{v}\cdot \vec{u}+{\cos }^2\frac{\theta }{2}\vec{v}+\sin \frac{\theta }{2}\cos \frac{\theta }{2}\vec{u}\times \vec{v}-\sin \frac{\theta }{2}\cos \frac{\theta }{2}\vec{v}\times \vec{u}-{\sin }^2\frac{\theta }{2}\vec{u}\times \vec{v}\times \vec{u}\\ & ={\sin }^2\frac{\theta }{2}(\vec{u}\cdot \vec{v})\cdot \vec{u}+{\cos }^2\frac{\theta }{2}\vec{v}+\sin \theta \vec{u}\times \vec{v}-{\sin }^2\frac{\theta }{2}[(\vec{u}\cdot \vec{u})\mathbf{v}-(\vec{v}\cdot \vec{u})\mathbf{u}]\\ & =\underline{{\sin }^2\frac{\theta }{2}(\vec{u}\cdot \vec{v})\cdot \mathbf{u}}+{\cos }^2\frac{\theta }{2}\mathbf{v}+\sin \theta \vec{u}\times \vec{v}-{\sin }^2\frac{\theta }{2}\mathbf{v}+\underline{{\sin }^2\frac{\theta }{2}(\vec{v}\cdot \vec{u})\mathbf{u}}\\ & =\cos \theta \mathbf{v}+2{\sin }^2\frac{\theta }{2}(\vec{v}\cdot \vec{u})\mathbf{u}+\sin \theta \vec{u}\times \vec{v}\\ & =\cos \theta \mathbf{v}+(1-\cos \theta )(\vec{v}\cdot \vec{u})\mathbf{u}+\sin \theta \vec{u}\times \vec{v}\end{array}\text{\hspace{1em}(3-8-7)}$$

注意：$\vec{u}$ 是单位向量，故 $\vec{u}\cdot \vec{u}=1$。

也就是拉格朗日公式结果。证毕。

CH4 李群与李代数

CH4-1 SO(3) 上的指数映射

将指数函数 $e^x$ 在 $x=0$ 处泰勒展开，即

$$\begin{array}{rl}{e}^x& =1+x+\frac{1}{2!}{x}^2+\frac{1}{3!}{x}^3+...+\frac{1}{n!}{x}^n\\ & =\sum _{n=0}^{\infty }\frac{x^n}{n!}\end{array}\text{\hspace{1em}(4-1-1)}$$

将矩阵 $\mathbf{A}$ 代入上式， 则

$$e^{\mathbf{A}}=\sum _{n=0}^{\infty }\frac{{\mathbf{A}}^n}{n!}$$

同样的，也有

$$e^{{\mathbf{\varphi }}^{\wedge }}=\sum _{n=0}^{\infty }\frac{({\mathbf{\varphi }}^{\wedge }{)}^n}{n!}\text{\hspace{1em}(4-1-2)}$$

令 $\mathbf{\varphi }=\theta \mathbf{a}$，$\theta$为模长，$\mathbf{a}$ 为单位方向向量。则上式可写为

$$e^{(\theta \mathbf{a}{)}^{\wedge }}=\sum _{n=0}^{\infty }\frac{(\theta {\mathbf{a}}^{\wedge }{)}^n}{n!}$$

我们知道

$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

则

$$\begin{array}{rl}{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }& =\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\\ & =\left[\begin{array}{ccc}-a_3^2-a_2^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_3^2-a_1^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_2^2-a_1^2\end{array}\right]\end{array}\text{\hspace{1em}(4-1-3)}$$

因为 $\mathbf{a}$ 是单位向量，则有 $a_1^2+a_2^2+a_3^2=1$，可得

$$\begin{array}{rl}\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]\left[\begin{array}{ccc}{a}_1& a_2& a_3\end{array}\right]& =\left[\begin{array}{ccc}{a}_1^2& a_1{a}_2& a_1{a}_3\\ a_2{a}_1& a_2^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& a_3^2\end{array}\right]-\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}-a_3^2-a_2^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_3^2-a_1^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_2^2-a_1^2\end{array}\right]\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\end{array}\text{\hspace{1em}(4-1-4)}$$

$$\begin{array}{rl}{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }& =\left[\begin{array}{ccc}-a_3^2-a_2^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_3^2-a_1^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_2^2-a_1^2\end{array}\right]\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\\ & =\left[\begin{array}{ccc}0& a_2^2{a}_3+a_3^3+a_1^2{a}_3& -a_2^3-a_2{a}_3^2-a_1{a}_2^2\\ -a_1^2{a}_3-a_3^3-a_1^2{a}_3& 0& a_1{a}_2^2+a_1^3+a_1{a}_3^2\\ a_2{a}_3^2+a_1^2{a}_2+a_2^3& -a_1{a}_3^2-a_1^3-a_1{a}_2^2& 0\end{array}\right]\end{array}$$

又 $a_1^2+a_2^2+a_3^2=1$，上式写为

$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& a_3& -a_2\\ -a_3& 0& a_1\\ a_2& -a_1& 0\end{array}\right]=-{\mathbf{a}}^{\wedge }\text{\hspace{1em}(4-1-5)}$$

对式（4-1-2）

$$\begin{array}{rl}{e}^{{\mathbf{\varphi }}^{\wedge }}=e^{(\theta \mathbf{a}{)}^{\wedge }}& =\sum _{n=0}^{\infty }\frac{(\theta {\mathbf{a}}^{\wedge }{)}^n}{n!}\\ & =\mathbf{I}+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{4!}{\theta }^4{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+...\\ & =(\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge })+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }-\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }-\frac{1}{4!}{\theta }^4{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+...\\ & =\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5+...){\mathbf{a}}^{\wedge }+(-1+\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+...){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =({\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I})+\sin \theta {\mathbf{a}}^{\wedge }-\cos \theta ({\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge })\\ & =(1-\cos \theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }\\ & =(1-\cos \theta )(\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I})+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }\\ & =\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}-\cos \theta \mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\cos \theta \mathbf{I}+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }\end{array}$$

于是得到李代数 $\mathbf{\varphi }$ 和旋转矩阵 $\mathbf{R}$ 之间的映射关系，即

$$\mathbf{R}=e^{{\mathbf{\varphi }}^{\wedge }}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }\text{\hspace{1em}(4-1-6)}$$

也就是 罗德里格斯公式。

CH4-2 SE(3) 上的指数映射

已知李代数 $\mathbf{\xi }=[\rho \varphi {]}^{\mathrm{T}}\in {\mathbb{R}}^6$，它的反对称矩阵为

$${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]$$

则李群为

$$\begin{array}{rl}\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge })& =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{({\mathbf{\varphi }}^{\wedge }{)}^n}{n!}& {\sum }_{n=0}^{\infty }\frac{({\mathbf{\varphi }}^{\wedge }{)}^n}{(n+1)!}\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\rho \\ 0^{\mathrm{T}}& 1\end{array}\right]\end{array}\text{\hspace{1em}(4-2-1)}$$

下面开始证明

同样，假设 $\mathbf{\varphi }=\theta \mathbf{a}$，$\theta$为模长，$\mathbf{a}$为单位方向向量。将 $\exp ({\mathbf{\xi }}^{\wedge })$ 泰勒展开

$$\exp ({\mathbf{\xi }}^{\wedge })=\frac{1}{n!}\sum _{n=0}^{\infty }{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^n=\frac{1}{n!}\sum _{n=0}^{\infty }{\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^n\text{\hspace{1em}(4-2-2)}$$

当 $n=0$ 时，

$$\frac{1}{0!}{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^0=\mathbf{I}$$

当 $n=1$ 时，

$$\frac{1}{1!}{\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^1=\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]$$

当 $n=2$ 时，

$$\frac{1}{2!}\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]=\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^2& \theta {\mathbf{a}}^{\wedge }\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]$$

当 $n=3$ 时，

$$\frac{1}{3!}\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]=\frac{1}{3!}\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^3& (\theta {\mathbf{a}}^{\wedge }{)}^2\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]$$

以此类推

$$\frac{1}{n!}{\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^n=\frac{1}{n!}\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^n& (\theta {\mathbf{a}}^{\wedge }{)}^{n-1}\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\text{\hspace{1em}(4-2-3)}$$

那么，式（4-1-8）可化为

$$\begin{array}{rl}\exp ({\mathbf{\xi }}^{\wedge })& =\frac{1}{n!}\sum _{n=0}^{\infty }{\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]}^n\\ & =\mathbf{I}+\left[\begin{array}{cc}\theta {\mathbf{a}}^{\wedge }& \rho \\ 0^{\mathrm{T}}& 0\end{array}\right]+\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^2& \theta {\mathbf{a}}^{\wedge }\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]+...+\frac{1}{n!}\left[\begin{array}{cc}(\theta {\mathbf{a}}^{\wedge }{)}^n& (\theta {\mathbf{a}}^{\wedge }{)}^{n-1}\rho \\ 0^{\mathrm{T}}& 0\end{array}\right]\\ & =\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\rho \\ 0^{\mathrm{T}}& 1\end{array}\right]\end{array}\text{\hspace{1em}(4-2-4)}$$

其中，左上角为 $SO(3)$ 指数映射，前面已经证明。令

$$\begin{array}{rl}\mathbf{J}& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =\mathbf{I}+\frac{1}{2!}\theta {\mathbf{a}}^{\wedge }+\frac{1}{3!}(\theta {\mathbf{a}}^{\wedge }{)}^2+\frac{1}{4!}(\theta {\mathbf{a}}^{\wedge }{)}^3+\frac{1}{5!}(\theta {\mathbf{a}}^{\wedge }{)}^4\\ & =\frac{1}{\theta }(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+...){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+...)({\mathbf{a}}^{\wedge }{)}^2+\mathbf{I}\\ & =\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }+\frac{\theta -\sin \theta }{\theta }({\mathbf{a}}^{\wedge }{)}^2+\mathbf{I}\\ & =\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }+(1-\frac{\sin \theta }{\theta })(\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I})+\mathbf{I}\\ & =\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}+\frac{\sin \theta }{\theta }\mathbf{I}+\mathbf{I}\\ & =\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\end{array}\text{\hspace{1em}(4-2-5)}$$

注意，这里用到式（4-1-5） $({\mathbf{a}}^{\wedge }{)}^3=-{\mathbf{a}}^{\wedge }$ 和 $\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }$ 以及泰勒展开

$$\cos \theta =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4+...$$

$$\sin \theta =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5+...$$

综上，证毕。

CH4-3 李代数求导

一、（1）$\mathrm{SO}(3)$ 直接求导

对极几何：本质矩阵奇异值分解

矩阵内积和迹

矩阵具有 弗罗比尼乌斯内积，类似向量的内积。它被定义为两个相同大小的矩阵 $\mathbf{A}$ 和 $\mathbf{B}$ 的对应元素的积的和， 即

$$<\mathbf{A},\mathbf{B}>=\sum _{i=1}^n\sum _{j=1}^n{a}_{ij}{b}_{ij}$$

以 $3\times 3$ 矩阵为例，设

$$\mathbf{A}=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]，\mathbf{B}=\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]$$

则有

$$<\mathbf{A},\mathbf{B}>=\sum _{i=1}^n\sum _{j=1}^n{a}_{ij}{b}_{ij}$$

对于 ${\mathbf{A}}^{\mathrm{T}}\mathbf{B}$

$${\mathbf{A}}^{\mathrm{T}}\mathbf{B}=\left[\begin{array}{ccc}{a}_{11}& a_{21}& a_{31}\\ a_{12}& a_{22}& a_{32}\\ a_{13}& a_{23}& a_{33}\end{array}\right]\left[\begin{array}{ccc}{b}_{11}& b_{12}& b_{13}\\ b_{21}& b_{22}& b_{23}\\ b_{31}& b_{32}& b_{33}\end{array}\right]=\left[\begin{array}{ccc}{a}_{11}{b}_{11}+a_{21}{b}_{21}+a_{31}{b}_{31}& ?& ?\\ ?& a_{12}{b}_{12}+a_{22}{b}_{22}+a_{32}{b}_{32}& ?\\ ?& ?& a_{13}{b}_{13}+a_{23}{b}_{23}+a_{33}{b}_{33}\end{array}\right]$$

则 $\mathrm{Tr}({\mathbf{A}}^{\mathrm{T}}\mathbf{B})$等于

$$\mathrm{Tr}({\mathbf{A}}^{\mathrm{T}}\mathbf{B})=a_{11}{b}_{11}+a_{21}{b}_{21}+a_{31}{b}_{31}+a_{12}{b}_{12}+a_{22}{b}_{22}+a_{32}{b}_{32}+a_{13}{b}_{13}+a_{23}{b}_{23}+a_{33}{b}_{33}=\sum _{i=1}^n\sum _{j=1}^n{a}_{ij}{b}_{ij}$$

也就是说 ${\mathbf{A}}^{\mathrm{T}}\mathbf{B}$ 的迹等于两矩阵对应元素相乘的积的和。