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

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

前面已经推导过

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

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

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

分别计算实部和虚部

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

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

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

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

证明：

a ⃗ × b ⃗ = a ∧ b = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ b 1 b 2 b 3 ] = △ T a b (3-8-4) \begin{aligned} \vec{a}\times\vec{b}&=\boldsymbol{a}^{\wedge}\boldsymbol{b} \\ &=\left[\begin{array}{c} 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{\bigtriangleup}=\boldsymbol{T}_a\boldsymbol{b} \end{aligned} \tag{3-8-4} a ×b ​=a∧b= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​ ​b1​b2​b3​​ ​=△Ta​b​(3-8-4)

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

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

而

T a T b = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 − b 3 b 2 b 3 0 − b 1 − b 2 b 1 0 ] = [ − 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 ] = − ( a ⃗ ⋅ b ⃗ ) I + b a T \begin{aligned} \boldsymbol{T}_a\boldsymbol{T}_b&=\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right]\left[\begin{array}{c} 0 & -b_3 & b_2 \\ b_3 & 0 & -b_1 \\ -b_2 & b_1 & 0 \end{array}\right] \\ &=\left[\begin{array}{c} -a_3b_3-a_2b_2 & a_2b_1 & a_3b_1 \\ a_1b_2 & -a_3b_3-a_1b_1 & a_3b_2 \\ a_1b_3 & a_2b_3 & -a_2b_2 -a_1b_1 \end{array}\right]\\ &=-(\vec{a}\cdot\vec{b})\boldsymbol{I}+\boldsymbol{b}\boldsymbol{a}^{\mathrm{T}} \end{aligned} Ta​Tb​​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​ ​0b3​−b2​​−b3​0b1​​b2​−b1​0​ ​= ​−a3​b3​−a2​b2​a1​b2​a1​b3​​a2​b1​−a3​b3​−a1​b1​a2​b3​​a3​b1​a3​b2​−a2​b2​−a1​b1​​ ​=−(a ⋅b )I+baT​

则（用到了矩阵结合律）

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

同理可证

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

证毕。

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

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

注意：$u$ 是单位向量，故 $u\cdot 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!}$$

我们知道

a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}=\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] a∧= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​

则

a ∧ a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = [ − 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 ] (4-1-3) \begin{aligned} \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}&=\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right]\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] \\ &=\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right] \end{aligned} \tag{4-1-3} a∧a∧​= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​ ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​= ​−a32​−a22​a1​a2​a1​a3​​a1​a2​−a32​−a12​a2​a3​​a1​a3​a2​a3​−a22​−a12​​ ​​(4-1-3)

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

a a T − I = [ a 1 a 2 a 3 ] [ a 1 a 2 a 3 ] = [ 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 ] − [ 1 0 0 0 1 0 0 0 1 ] = [ − 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 ] = a ∧ a ∧ (4-1-4) \begin{aligned} \boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{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}{c} a_1^2 & a_1a_2 & a_1a_3 \\ a_2a_1 & a_2^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & a_3^2 \end{array}\right]- \left[\begin{array}{c} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right] \\ &=\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right] \\ &=\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} \end{aligned} \tag{4-1-4} aaT−I= ​a1​a2​a3​​ ​[a1​​a2​​a3​​]​= ​a12​a2​a1​a1​a3​​a1​a2​a22​a2​a3​​a1​a3​a2​a3​a32​​ ​− ​100​010​001​ ​= ​−a32​−a22​a1​a2​a1​a3​​a1​a2​−a32​−a12​a2​a3​​a1​a3​a2​a3​−a22​−a12​​ ​=a∧a∧​(4-1-4)

a ∧ a ∧ a ∧ = [ − 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 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = [ 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 ] \begin{aligned} \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}&=\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right]\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] \\ &=\left[\begin{array}{c} 0 & a_2^2a_3+a_3^3+a_1^2a_3 & -a_2^3-a_2a_3^2-a_1a_2^2 \\ -a_1^2a_3-a_3^3-a_1^2a_3 & 0 & a_1a_2^2+a_1^3+a_1a_3^2 \\ a_2a_3^2+a_1^2a_2+a_2^3 & -a_1a_3^2-a_1^3-a_1a_2^2 & 0 \end{array}\right] \\ \end{aligned} a∧a∧a∧​= ​−a32​−a22​a1​a2​a1​a3​​a1​a2​−a32​−a12​a2​a3​​a1​a3​a2​a3​−a22​−a12​​ ​ ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​= ​0−a12​a3​−a33​−a12​a3​a2​a32​+a12​a2​+a23​​a22​a3​+a33​+a12​a3​0−a1​a32​−a13​−a1​a22​​−a23​−a2​a32​−a1​a22​a1​a22​+a13​+a1​a32​0​ ​​

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

a ∧ a ∧ a ∧ = [ 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0 ] = − a ∧ (4-1-5) \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=\left[\begin{array}{c} 0 & a_3 & -a_2 \\ -a_3 & 0 & a_1 \\ a_2 & -a_1 & 0 \end{array}\right]=-\boldsymbol{a}^{\wedge} \tag{4-1-5} a∧a∧a∧= ​0−a3​a2​​a3​0−a1​​−a2​a1​0​ ​=−a∧(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$ 矩阵为例，设

A = [ a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 ] ， B = [ b 11 b 12 b 13 b 21 b 22 b 23 b 31 b 32 b 33 ] \boldsymbol{A}=\left[\begin{array}{c} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{array}\right]， \boldsymbol{B}=\left[\begin{array}{c} b_{11} & b_{12} & b_{13} \\ b_{21} & b_{22} & b_{23} \\ b_{31} & b_{32} & b_{33} \end{array}\right] A= ​a11​a21​a31​​a12​a22​a32​​a13​a23​a33​​ ​，B= ​b11​b21​b31​​b12​b22​b32​​b13​b23​b33​​ ​

则有

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

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

A T B = [ a 11 a 21 a 31 a 12 a 22 a 32 a 13 a 23 a 33 ] [ b 11 b 12 b 13 b 21 b 22 b 23 b 31 b 32 b 33 ] = [ 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 ] \boldsymbol{A}^{\mathrm{T}}\boldsymbol{B}=\left[\begin{array}{c} a_{11} & a_{21} & a_{31} \\ a_{12} & a_{22} & a_{32} \\ a_{13} & a_{23} & a_{33} \end{array}\right] \left[\begin{array}{c} b_{11} & b_{12} & b_{13} \\ b_{21} & b_{22} & b_{23} \\ b_{31} & b_{32} & b_{33} \end{array}\right]= \left[\begin{array}{c} 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] ATB= ​a11​a12​a13​​a21​a22​a23​​a31​a32​a33​​ ​ ​b11​b21