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

公式推导（三）"/>

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

文章目录

• • • • CH3-8 证明旋转后的四元数虚部为零，实部为罗德里格斯公式结果
    • CH4 李群与李代数
    • • CH4-1 SO(3) 上的指数映射
      • CH4-2 SE(3) 上的指数映射
      • CH4-3 李代数求导
      • 对极几何：本质矩阵奇异值分解
      • 矩阵内积和迹

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

前面已经推导过

v ′ = p v p ∗ = p v p − 1 v'=pvp^*=pvp^{-1} v′=pvp∗=pvp−1

其中， v = [ 0 , v ⃗ ] v=[0,\vec{v}] v=[0,v ]， p = [ cos ⁡ θ 2 , sin ⁡ θ 2 u ⃗ ] p=[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\vec{u}] p=[cos2θ​,sin2θ​u ]，代入上式

v ′ = p v p ∗ = [ cos ⁡ θ 2 , sin ⁡ θ 2 u ⃗ ] [ 0 , v ⃗ ] [ cos ⁡ θ 2 , − sin ⁡ θ 2 u ⃗ ] = [ 0 − sin ⁡ θ 2 u ⃗ ⋅ v ⃗ , cos ⁡ θ 2 v ⃗ + 0 + sin ⁡ θ 2 u ⃗ × v ⃗ ] [ cos ⁡ θ 2 , − sin ⁡ θ 2 u ⃗ ] = [ − sin ⁡ θ 2 u ⃗ ⋅ v ⃗ , cos ⁡ θ 2 v ⃗ + sin ⁡ θ 2 u ⃗ × v ⃗ ] [ cos ⁡ θ 2 , − sin ⁡ θ 2 u ⃗ ] (3-8-1) \begin{aligned} v'&=pvp^* \\ &=[\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{aligned} \tag{3-8-1} v′​=pvp∗=[cos2θ​,sin2θ​u ][0,v ][cos2θ​,−sin2θ​u ]=[0−sin2θ​u ⋅v ,cos2θ​v +0+sin2θ​u ×v ][cos2θ​,−sin2θ​u ]=[−sin2θ​u ⋅v ,cos2θ​v +sin2θ​u ×v ][cos2θ​,−sin2θ​u ]​(3-8-1)

分别计算实部和虚部

R e = − sin ⁡ θ 2 cos ⁡ θ 2 u ⃗ ⋅ v ⃗ + ( cos ⁡ θ 2 v ⃗ + sin ⁡ θ 2 u ⃗ × v ⃗ ) ⋅ sin ⁡ θ 2 u ⃗ = − sin ⁡ θ 2 cos ⁡ θ 2 u ⃗ ⋅ v ⃗ + cos ⁡ θ 2 sin ⁡ θ 2 u ⃗ ⋅ v ⃗ + sin ⁡ θ 2 ( u ⃗ × v ⃗ ) ⋅ u ⃗ = 0 + 0 = 0 (3-8-2) \begin{aligned} \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{aligned} \tag{3-8-2} Re​=−sin2θ​cos2θ​u ⋅v +(cos2θ​v +sin2θ​u ×v )⋅sin2θ​u =−sin2θ​cos2θ​u ⋅v +cos2θ​sin2θ​u ⋅v +sin2θ​(u ×v )⋅u =0+0=0​(3-8-2)

I m = ( − sin ⁡ θ 2 u ⃗ ⋅ v ⃗ ) ⋅ ( − sin ⁡ θ 2 u ⃗ ) + ( cos ⁡ θ 2 v ⃗ + sin ⁡ θ 2 u ⃗ × v ⃗ ) cos ⁡ θ 2 + ( cos ⁡ θ 2 v ⃗ + sin ⁡ θ 2 u ⃗ × v ⃗ ) × ( − sin ⁡ θ 2 u ⃗ ) (3-8-3) \begin{aligned} \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{aligned} \tag{3-8-3} Im​=(−sin2θ​u ⋅v )⋅(−sin2θ​u )+(cos2θ​v +sin2θ​u ×v )cos2θ​+(cos2θ​v +sin2θ​u ×v )×(−sin2θ​u )​(3-8-3)

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

先证明公式： a ⃗ × ( b ⃗ × c ⃗ ) = ( a ⃗ ⋅ c ⃗ ) ⋅ b ⃗ − ( a ⃗ ⋅ b ⃗ ) ⋅ c ⃗ \vec{a}\times(\vec{b}\times\vec{c})=(\vec{a}\cdot\vec{c})\cdot\vec{b}-(\vec{a}\cdot\vec{b})\cdot\vec{c} a ×(b ×c )=(a ⋅c )⋅b −(a ⋅b )⋅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 ⃗ × ( b ⃗ × c ⃗ ) = ( T a T b ) c = T a T b c \vec{a}\times(\vec{b}\times\vec{c})=(\boldsymbol{T}_a\boldsymbol{T}_b)\boldsymbol{c}=\boldsymbol{T}_a\boldsymbol{T}_b\boldsymbol{c} a ×(b ×c )=(Ta​Tb​)c=Ta​Tb​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​

则（用到了矩阵结合律）

a ⃗ × ( b ⃗ × c ⃗ ) = T a T b c = ( − ( a ⃗ ⋅ b ⃗ ) I + b a T ) c = − ( a ⃗ ⋅ b ⃗ ) c + b ( a T c ) = − ( a ⃗ ⋅ b ⃗ ) c + ( a ⃗ ⋅ c ⃗ ) b (3-8-5) \begin{aligned} \vec{a}\times(\vec{b}\times\vec{c})=\boldsymbol{T}_a\boldsymbol{T}_b\boldsymbol{c}&=(-(\vec{a}\cdot\vec{b})\boldsymbol{I}+\boldsymbol{b}\boldsymbol{a}^{\mathrm{T}})\boldsymbol{c} \\ &=-(\vec{a}\cdot\vec{b})\boldsymbol{c}+\boldsymbol{b}(\boldsymbol{a}^{\mathrm{T}}\boldsymbol{c}) \\ &=-(\vec{a}\cdot\vec{b})\boldsymbol{c}+(\vec{a}\cdot\vec{c})\boldsymbol{b} \end{aligned} \tag{3-8-5} a ×(b ×c )=Ta​Tb​c​=(−(a ⋅b )I+baT)c=−(a ⋅b )c+b(aTc)=−(a ⋅b )c+(a ⋅c )b​(3-8-5)

同理可证

( a ⃗ × b ⃗ ) × c ⃗ = ( a ⃗ ⋅ c ⃗ ) b − ( b ⃗ ⋅ c ⃗ ) a (3-8-6) (\vec{a}\times\vec{b})\times\vec{c}=(\vec{a}\cdot\vec{c})\boldsymbol{b}-(\vec{b}\cdot\vec{c})\boldsymbol{a} \tag{3-8-6} (a ×b )×c =(a ⋅c )b−(b ⋅c )a(3-8-6)

证毕。

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

I m = sin ⁡ 2 θ 2 u ⃗ ⋅ v ⃗ ⋅ u ⃗ + cos ⁡ 2 θ 2 v ⃗ + sin ⁡ θ 2 cos ⁡ θ 2 u ⃗ × v ⃗ − sin ⁡ θ 2 cos ⁡ θ 2 v ⃗ × u ⃗ − sin ⁡ 2 θ 2 u ⃗ × v ⃗ × u ⃗ = sin ⁡ 2 θ 2 ( u ⃗ ⋅ v ⃗ ) ⋅ u ⃗ + cos ⁡ 2 θ 2 v ⃗ + sin ⁡ θ u ⃗ × v ⃗ − sin ⁡ 2 θ 2 [ ( u ⃗ ⋅ u ⃗ ) v − ( v ⃗ ⋅ u ⃗ ) u ] = sin ⁡ 2 θ 2 ( u ⃗ ⋅ v ⃗ ) ⋅ u ‾ + cos ⁡ 2 θ 2 v + sin ⁡ θ u ⃗ × v ⃗ − sin ⁡ 2 θ 2 v + sin ⁡ 2 θ 2 ( v ⃗ ⋅ u ⃗ ) u ‾ = cos ⁡ θ v + 2 sin ⁡ 2 θ 2 ( v ⃗ ⋅ u ⃗ ) u + sin ⁡ θ u ⃗ × v ⃗ = cos ⁡ θ v + ( 1 − cos ⁡ θ ) ( v ⃗ ⋅ u ⃗ ) u + sin ⁡ θ u ⃗ × v ⃗ (3-8-7) \begin{aligned} \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})\boldsymbol{v}-(\vec{v}\cdot\vec{u})\boldsymbol{u}] \\ &=\underline{\sin^2\frac{\theta}{2}(\vec{u}\cdot\vec{v})\cdot\boldsymbol{u}}+\cos^2\frac{\theta}{2}\boldsymbol{v}+\sin\theta\vec{u}\times\vec{v}-\sin^2\frac{\theta}{2}\boldsymbol{v}+\underline{\sin^2\frac{\theta}{2}(\vec{v}\cdot\vec{u})\boldsymbol{u}} \\ &=\cos\theta\boldsymbol{v}+2\sin^2\frac{\theta}{2}(\vec{v}\cdot\vec{u})\boldsymbol{u}+\sin\theta\vec{u}\times\vec{v} \\ &=\cos\theta\boldsymbol{v}+(1-\cos\theta)(\vec{v}\cdot\vec{u})\boldsymbol{u}+\sin\theta\vec{u}\times\vec{v} \end{aligned} \tag{3-8-7} Im​=sin22θ​u ⋅v ⋅u +cos22θ​v +sin2θ​cos2θ​u ×v −sin2θ​cos2θ​v ×u −sin22θ​u ×v ×u =sin22θ​(u ⋅v )⋅u +cos22θ​v +sinθu ×v −sin22θ​[(u ⋅u )v−(v ⋅u )u]=sin22θ​(u ⋅v )⋅u​+cos22θ​v+sinθu ×v −sin22θ​v+sin22θ​(v ⋅u )u​=cosθv+2sin22θ​(v ⋅u )u+sinθu ×v =cosθv+(1−cosθ)(v ⋅u )u+sinθu ×v ​(3-8-7)

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

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

CH4 李群与李代数

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

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

e x = 1 + x + 1 2 ! x 2 + 1 3 ! x 3 + . . . + 1 n ! x n = ∑ n = 0 ∞ x n n ! (4-1-1) \begin{aligned} 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{aligned} \tag{4-1-1} ex​=1+x+2!1​x2+3!1​x3+...+n!1​xn=n=0∑∞​n!xn​​(4-1-1)

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

e A = ∑ n = 0 ∞ A n n ! e^{\boldsymbol{A}}=\sum_{n=0}^{\infty}\frac{\boldsymbol{A}^n}{n!} eA=n=0∑∞​n!An​

同样的，也有

e ϕ ∧ = ∑ n = 0 ∞ ( ϕ ∧ ) n n ! (4-1-2) e^{\boldsymbol{\phi}^{\wedge}}=\sum_{n=0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{n!} \tag{4-1-2} eϕ∧=n=0∑∞​n!(ϕ∧)n​(4-1-2)

令 ϕ = θ a \boldsymbol{\phi}=\theta\boldsymbol{a} ϕ=θa， θ \theta θ为模长， a \boldsymbol{a} a 为单位方向向量。则上式可写为

e ( θ a ) ∧ = ∑ n = 0 ∞ ( θ a ∧ ) n n ! e^{\boldsymbol({\theta\boldsymbol{a}})^{\wedge}}=\sum_{n=0}^{\infty}\frac{(\theta\boldsymbol{a}^{\wedge})^n}{n!} e(θa)∧=n=0∑∞​n!(θa∧)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)

因为 a \boldsymbol{a} a 是单位向量，则有 a 1 2 + a 2 2 + a 3 2 = 1 a_1^2+a_2^2+a_3^2=1 a12​+a22​+a32​=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_1^2+a_2^2+a_3^2=1 a12​+a22​+a32​=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）

e ϕ ∧ = e ( θ a ) ∧ = ∑ n = 0 ∞ ( θ a ∧ ) n n ! = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 a ∧ a ∧ a ∧ a ∧ + . . . = ( a a T − a ∧ a ∧ ) + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 a ∧ a ∧ + . . . = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 + . . . ) a ∧ + ( − 1 + 1 2 ! θ 2 − 1 4 ! θ 4 + . . . ) a ∧ a ∧ = ( a ∧ a ∧ + I ) + sin ⁡ θ a ∧ − cos ⁡ θ ( a ∧ a ∧ ) = ( 1 − cos ⁡ θ ) a ∧ a ∧ + I + sin ⁡ θ a ∧ = ( 1 − cos ⁡ θ ) ( a a T − I ) + I + sin ⁡ θ a ∧ = a a T − I − cos ⁡ θ a a T + cos ⁡ θ I + I + sin ⁡ θ a ∧ = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ \begin{aligned} e^{\boldsymbol{\phi}^{\wedge}}=e^{\boldsymbol({\theta\boldsymbol{a}})^{\wedge}}&=\sum_{n=0}^{\infty}\frac{(\theta\boldsymbol{a}^{\wedge})^n}{n!} \\ &=\boldsymbol{I}+\theta\boldsymbol{a}^{\wedge}+\frac{1}{2!}\theta^2 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\frac{1}{3!}\theta^3 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\frac{1}{4!}\theta^4 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+...\\ &=(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge})+\theta\boldsymbol{a}^{\wedge}+\frac{1}{2!}\theta^2 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}-\frac{1}{3!}\theta^3 \boldsymbol{a}^{\wedge}-\frac{1}{4!}\theta^4 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+...\\ &=\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5+...)\boldsymbol{a}^{\wedge}+(-1+\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\\ &=(\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\boldsymbol{I})+\sin\theta \boldsymbol{a}^{\wedge}-\cos\theta(\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge})\\ &=(1-\cos\theta)\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin\theta \boldsymbol{a}^{\wedge} \\ &=(1-\cos\theta)(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I})+\boldsymbol{I}+\sin\theta \boldsymbol{a}^{\wedge}\\ &=\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}-\cos\theta\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\cos\theta\boldsymbol{I}+\boldsymbol{I}+\sin\theta \boldsymbol{a}^{\wedge}\\ &=\cos\theta\boldsymbol{I}+(1-\cos\theta)\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\sin\theta \boldsymbol{a}^{\wedge} \end{aligned} eϕ∧=e(θa)∧​=n=0∑∞​n!(θa∧)n​=I+θa∧+2!1​θ2a∧a∧+3!1​θ3a∧a∧a∧+4!1​θ4a∧a∧a∧a∧+...=(aaT−a∧a∧)+θa∧+2!1​θ2a∧a∧−3!1​θ3a∧−4!1​θ4a∧a∧+...=aaT+(θ−3!1​θ3+5!1​θ5+...)a∧+(−1+2!1​θ2−4!1​θ4+...)a∧a∧=(a∧a∧+I)+sinθa∧−cosθ(a∧a∧)=(1−cosθ)a∧a∧+I+sinθa∧=(1−cosθ)(aaT−I)+I+sinθa∧=aaT−I−cosθaaT+cosθI+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧​

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

R = e ϕ ∧ = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ (4-1-6) \boldsymbol{R}=e^{\boldsymbol{\phi}^{\wedge}}=\cos\theta\boldsymbol{I}+(1-\cos\theta)\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\sin\theta \boldsymbol{a}^{\wedge} \tag{4-1-6} R=eϕ∧=cosθI+(1−cosθ)aaT+sinθa∧(4-1-6)

也就是 罗德里格斯公式。

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

已知李代数 ξ = [ ρ ϕ ] T ∈ R 6 \boldsymbol{\xi}=[\rho \quad \phi]^{\mathrm{T}}\in \boldsymbol{\mathbb{R}}^6 ξ=[ρϕ]T∈R6，它的反对称矩阵为

ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] ξ∧=[ϕ∧0T​ρ0​]

则李群为

T = exp ⁡ ( ξ ∧ ) = [ ∑ n = 0 ∞ ( ϕ ∧ ) n n ! ∑ n = 0 ∞ ( ϕ ∧ ) n ( n + 1 ) ! ρ 0 T 0 ] ≜ [ R J ρ 0 T 1 ] (4-2-1) \begin{aligned} \boldsymbol{T}=\exp(\boldsymbol{\xi}^{\wedge})&=\left[\begin{array}{c} \sum_{n=0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{n!} & \sum_{n=0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{(n+1)!}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] \\ &\triangleq \left[\begin{array}{c} \boldsymbol{R} & \boldsymbol{J}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] \end{aligned} \tag{4-2-1} T=exp(ξ∧)​=[∑n=0∞​n!(ϕ∧)n​0T​∑n=0∞​(n+1)!(ϕ∧)n​ρ0​]≜[R0T​Jρ1​]​(4-2-1)

下面开始证明

同样，假设 ϕ = θ a \boldsymbol{\phi}=\theta\boldsymbol{a} ϕ=θa， θ \theta θ为模长， a \boldsymbol{a} a为单位方向向量。将 exp ⁡ ( ξ ∧ ) \exp(\boldsymbol{\xi}^{\wedge}) exp(ξ∧) 泰勒展开

exp ⁡ ( ξ ∧ ) = 1 n ! ∑ n = 0 ∞ [ ϕ ∧ ρ 0 T 0 ] n = 1 n ! ∑ n = 0 ∞ [ θ a ∧ ρ 0 T 0 ] n (4-2-2) \exp(\boldsymbol{\xi}^{\wedge})=\frac{1}{n!}\sum_{n=0}^{\infty}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n=\frac{1}{n!}\sum_{n=0}^{\infty}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n \tag{4-2-2} exp(ξ∧)=n!1​n=0∑∞​[ϕ∧0T​ρ0​]n=n!1​n=0∑∞​[θa∧0T​ρ0​]n(4-2-2)

当 n = 0 n=0 n=0 时，

1 0 ! [ ϕ ∧ ρ 0 T 0 ] 0 = I \frac{1}{0!}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^0=\boldsymbol{I} 0!1​[ϕ∧0T​ρ0​]0=I

当 n = 1 n=1 n=1 时，

1 1 ! [ θ a ∧ ρ 0 T 0 ] 1 = [ θ a ∧ ρ 0 T 0 ] \frac{1}{1!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^1=\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 1!1​[θa∧0T​ρ0​]1=[θa∧0T​ρ0​]

当 n = 2 n=2 n=2 时，

1 2 ! [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] = [ ( θ a ∧ ) 2 θ a ∧ ρ 0 T 0 ] \frac{1}{2!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]=\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^2 & \theta\boldsymbol{a}^{\wedge}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 2!1​[θa∧0T​ρ0​][θa∧0T​ρ0​]=[(θa∧)20T​θa∧ρ0​]

当 n = 3 n=3 n=3 时，

1 3 ! [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] = 1 3 ! [ ( θ a ∧ ) 3 ( θ a ∧ ) 2 ρ 0 T 0 ] \frac{1}{3!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]=\frac{1}{3!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^3 & (\theta\boldsymbol{a}^{\wedge})^2\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 3!1​[θa∧0T​ρ0​][θa∧0T​ρ0​][θa∧0T​ρ0​]=3!1​[(θa∧)30T​(θa∧)2ρ0​]

以此类推

1 n ! [ θ a ∧ ρ 0 T 0 ] n = 1 n ! [ ( θ a ∧ ) n ( θ a ∧ ) n − 1 ρ 0 T 0 ] (4-2-3) \frac{1}{n!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n=\frac{1}{n!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^n & (\theta\boldsymbol{a}^{\wedge})^{n-1}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] \tag{4-2-3} n!1​[θa∧0T​ρ0​]n=n!1​[(θa∧)n0T​(θa∧)n−1ρ0​](4-2-3)

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

exp ⁡ ( ξ ∧ ) = 1 n ! ∑ n = 0 ∞ [ ϕ ∧ ρ 0 T 0 ] n = I + [ θ a ∧ ρ 0 T 0 ] + [ ( θ a ∧ ) 2 θ a ∧ ρ 0 T 0 ] + . . . + 1 n ! [ ( θ a ∧ ) n ( θ a ∧ ) n − 1 ρ 0 T 0 ] = [ ∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n ρ 0 T 1 ] (4-2-4) \begin{aligned} \exp(\boldsymbol{\xi}^{\wedge})&=\frac{1}{n!}\sum_{n=0}^{\infty}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n \\ &=\boldsymbol{I}+\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]+\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^2 & \theta\boldsymbol{a}^{\wedge}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]+...+\frac{1}{n!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^n & (\theta\boldsymbol{a}^{\wedge})^{n-1}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\\ &=\left[\begin{array}{c} \sum_{n=0}^{\infty}\frac{1}{n!}(\theta\boldsymbol{a}^{\wedge})^n & \sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\theta\boldsymbol{a}^{\wedge})^n\rho \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] \tag{4-2-4} \end{aligned} exp(ξ∧)​=n!1​n=0∑∞​[ϕ∧0T​ρ0​]n=I+[θa∧0T​ρ0​]+[(θa∧)20T​θa∧ρ0​]+...+n!1​[(θa∧)n0T​(θa∧)n−1ρ0​]=[∑n=0∞​n!1​(θa∧)n0T​∑n=0∞​(n+1)!1​(θa∧)nρ1​]​(4-2-4)

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

J = ∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n = I + 1 2 ! θ a ∧ + 1 3 ! ( θ a ∧ ) 2 + 1 4 ! ( θ a ∧ ) 3 + 1 5 ! ( θ a ∧ ) 4 = 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + . . . ) a ∧ + 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 + . . . ) ( a ∧ ) 2 + I = 1 − cos ⁡ θ θ a ∧ + θ − sin ⁡ θ θ ( a ∧ ) 2 + I = 1 − cos ⁡ θ θ a ∧ + ( 1 − sin ⁡ θ θ ) ( a a T − I ) + I = 1 − cos ⁡ θ θ a ∧ + ( 1 − sin ⁡ θ θ ) a a T − I + sin ⁡ θ θ I + I = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ (4-2-5) \begin{aligned} \boldsymbol{J}&=\sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\theta\boldsymbol{a}^{\wedge})^n \\ &=\boldsymbol{I}+\frac{1}{2!}\theta\boldsymbol{a}^{\wedge}+\frac{1}{3!}(\theta\boldsymbol{a}^{\wedge})^2+\frac{1}{4!}(\theta\boldsymbol{a}^{\wedge})^3+\frac{1}{5!}(\theta\boldsymbol{a}^{\wedge})^4 \\ &=\frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4+...)\boldsymbol{a}^{\wedge}+\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+...)(\boldsymbol{a}^{\wedge})^2+\boldsymbol{I} \\ &=\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}+\frac{\theta-\sin\theta}{\theta}(\boldsymbol{a}^{\wedge})^2+\boldsymbol{I}\\ &=\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}+(1-\frac{\sin\theta}{\theta})(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I})+\boldsymbol{I}\\ &=\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}+(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}+\frac{\sin\theta}{\theta}\boldsymbol{I}+\boldsymbol{I}\\ &=\frac{\sin\theta}{\theta}\boldsymbol{I}+(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}+\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge} \tag{4-2-5} \end{aligned} J​=n=0∑∞​(n+1)!1​(θa∧)n=I+2!1​θa∧+3!1​(θa∧)2+4!1​(θa∧)3+5!1​(θa∧)4=θ1​(2!1​θ2−4!1​θ4+...)a∧+θ1​(3!1​θ3−5!1​θ5+...)(a∧)2+I=θ1−cosθ​a∧+θθ−sinθ​(a∧)2+I=θ1−cosθ​a∧+(1−θsinθ​)(aaT−I)+I=θ1−cosθ​a∧+(1−θsinθ​)aaT−I+θsinθ​I+I=θsinθ​I+(1−θsinθ​)aaT+θ1−cosθ​a∧​(4-2-5)

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

cos ⁡ θ = 1 − 1 2 ! θ 2 + 1 4 ! θ 4 + . . . \cos\theta=1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4+... cosθ=1−2!1​θ2+4!1​θ4+...

sin ⁡ θ = θ − 1 3 ! θ 3 + 1 5 ! θ 5 + . . . \sin\theta=\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5+... sinθ=θ−3!1​θ3+5!1​θ5+...

综上，证毕。

CH4-3 李代数求导

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

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

矩阵内积和迹

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

< A , B > = ∑ i = 1 n ∑ j = 1 n a i j b i j <\boldsymbol{A},\boldsymbol{B}>=\sum_{i=1}^{n}\sum_{j=1}^na_{ij}b_{ij} <A,B>=i=1∑n​j=1∑n​aij​bij​

以 3 × 3 3\times 3 3×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​​ ​

则有

< A , B > = ∑ i = 1 n ∑ j = 1 n a i j b i j <\boldsymbol{A},\boldsymbol{B}>=\sum_{i=1}^{n}\sum_{j=1}^na_{ij}b_{ij} <A,B>=i=1∑n​j=1∑n​aij​bij​

对于 A T B \boldsymbol{A}^{\mathrm{T}}\boldsymbol{B} ATB

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​b31​​b12​b22​b32​​b13​b23​b33​​ ​= ​a11​b11​+a21​b21​+a31​b31​??​?a12​b12​+a22​b22​+a32​b32​?​??a13​b13​+a23​b23​+a33​b33​​ ​

则 T r ( A T B ) \mathrm{Tr}(\boldsymbol{A}^{\mathrm{T}}\boldsymbol{B}) Tr(ATB)等于

T r ( A T 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 = ∑ i = 1 n ∑ j = 1 n a i j b i j \mathrm{Tr}(\boldsymbol{A}^{\mathrm{T}}\boldsymbol{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}^na_{ij}b_{ij} Tr(ATB)=a11​b11​+a21​b21​+a31​b31​+a12​b12​+a22​b22​+a32​b32​+a13​b13​+a23​b23​+a33​b33​=i=1∑n​j=1∑n​aij​bij​

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