- 前言
- 坐标，基与旋转矩阵
- 内积和外积
- 变换矩阵
- 旋转矩阵与变换矩阵的自由度
- 旋转向量与旋转角
- 欧拉角
- 四元数
- 单位四元数表示旋转
- 后记
视觉SLAM十四讲这本书我应该已经看过四五遍了。前两遍看起来比较吃力，因为SLAM涉及到了比较广泛的数学理论，包括线性代数、概率论、随机过程、优化理论等。
因此我先将SLAM相关的一些数学基础在数值计算专栏里总结下来。据说高博曾经说过：数学学不好，Demo跑到老~笑:）。数学能力过关后，再回到十四讲这本书，看起来就有种说不出的爽感，越读越有劲。
本篇将开始进行SLAM十四讲的学习和总结。
坐标，基与旋转矩阵在一个标准正交基下的向量可以通过坐标表示为： α = [ e ⃗ 1 , e ⃗ 2 , e ⃗ 3 ] [ a 1 , a 2 , a 3 ] T \alpha =[\vec e_1, \vec e_2, \vec e_3][a_1,a_2,a_3]^T α=[e 1,e 2,e 3][a1,a2,a3]T 同一向量在不同基下的坐标可表示为： [ e ⃗ 1 , e ⃗ 2 , e ⃗ 3 ] [ a 1 , a 2 , a 3 ] T = [ e ⃗ 1 ′ , e ⃗ 2 ′ , e ⃗ 3 ′ ] [ a 1 ′ , a 2 ′ , a 3 ′ ] T [ a 1 ′ , a 2 ′ , a 3 ′ ] T = [ e ⃗ 1 ′ , e ⃗ 2 ′ , e ⃗ 3 ′ ] T [ e ⃗ 1 , e ⃗ 2 , e ⃗ 3 ] [ a 1 , a 2 , a 3 ] T a ′ = R a [\vec e_1, \vec e_2, \vec e_3][a_1,a_2,a_3]^T=[\vec e_1', \vec e_2', \vec e_3'][a'_1,a'_2,a'_3]^T \\ \quad \\ [a'_1,a'_2,a'_3]^T= [\vec e_1', \vec e_2', \vec e_3']^T[\vec e_1, \vec e_2, \vec e_3][a_1,a_2,a_3]^T \\ \quad \\ a' = Ra [e 1,e 2,e 3][a1,a2,a3]T=[e 1′,e 2′,e 3′][a1′,a2′,a3′]T[a1′,a2′,a3′]T=[e 1′,e 2′,e 3′]T[e 1,e 2,e 3][a1,a2,a3]Ta′=Ra 上式中的 R R R被称为从坐标系 ⊥ a → ⊥ a ′ \bot_a\to \bot'_a ⊥a→⊥a′的旋转矩阵。
从线性代数上而言， a ′ = R a a'=Ra a′=Ra表示坐标 a a a通过 R R R线性变换为坐标 a ′ a' a′。两个标准正交基的乘积还是标准正交基，因此旋转矩阵 R R R是正交矩阵。并且旋转矩阵的行列式必为1， R T R^T RT表示一个旋转 R R R的反旋转。
通常把机器人自身作为可变的移动坐标系，外界环境中存在一个不变的世界坐标系。
旋转矩阵表示的是不同坐标系之间，相应坐标轴的旋转关系。
内积和外积内积： a ⋅ b = a T b = ∣ a ∣ ∣ b ∣ c o s θ a b a\cdot b=a^Tb=|a||b|cos\theta_{ab} a⋅b=aTb=∣a∣∣b∣cosθab
外积： a × b = ∣ i j k a 1 a 2 a 3 b 1 b 2 b 3 ∣ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ∧ b a\times b= \begin{vmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \\ \end{vmatrix}=\begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}b=a^\land b a×b=∣∣∣∣∣∣ia1b1ja2b2ka3b3∣∣∣∣∣∣=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b 在这里使用 a ∧ a^\land a∧表示向量与矩阵的一种映射关系，简化运算。
变换矩阵两个坐标系之间的旋转和位移可表示为： a ′ = R a + t a'=Ra+t a′=Ra+t 称为欧式变换。可以使用齐次坐标简化欧式变换的表现形式： [ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] 简 记 为 ： a ′ = T a \begin{bmatrix} a' \\ 1 \\ \end{bmatrix}= \begin{bmatrix} R & t \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} a \\ 1 \\ \end{bmatrix} \\ \quad \\ 简记为：a' = Ta [a′1]=[R0t1][a1]简记为：a′=Ta 将 T T T称为变换矩阵，包含一个旋转矩阵和一个位移向量。可以使用 T − 1 T^{-1} T−1表示一个反变换： T − 1 = [ R T − R T t 0 T 1 ] T^{-1}=\begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \\ \end{bmatrix} T−1=[RT0T−RTt1]
旋转矩阵与变换矩阵的自由度三维空间中的旋转具有3个自由度，而旋转矩阵有9个元素；欧式变换具有6个自由度，而变换矩阵有16个元素。此外，旋转矩阵的行列式为1。
使用旋转矩阵和变换矩阵不仅计算冗余，还受到行列式约束。而计算冗余和约束都是由旋转矩阵带来的。
旋转向量与旋转角三维空间中的旋转也可以表示为，向量绕着某一个单位向量旋转一个具体角度： ϕ = θ n \phi = \theta n ϕ=θn 即使用轴角angle-axis，其中 θ \theta θ表示旋转角度， n n n表示单位旋转向量。
罗德里格斯公式：旋转向量到旋转矩阵 R = cos θ I + ( 1 − cos θ ) n n T + s i n θ n ∧ R=\cos\theta I+(1-\cos \theta)nn^T+sin\theta n^\land R=cosθI+(1−cosθ)nnT+sinθn∧
旋转矩阵到旋转向量： t r ( R ) = 3 cos θ + ( 1 − cos θ ) t r ( n n T ) = 3 cos θ + ( 1 − cos θ ) ∣ ∣ n ∣ ∣ 2 2 = 3 cos θ + ( 1 − cos θ ) = 1 + 2 cos θ θ = arccos t r ( R ) − 1 2 R n = n , ( R − I ) n = 0 , ∣ ∣ n ∣ ∣ 2 = 1 tr(R)=3\cos \theta+(1-\cos \theta)tr(nn^T) \\ =3\cos \theta+(1-\cos \theta)||n||_2^2 \\ =3\cos \theta+(1-\cos \theta)=1+2\cos \theta \\ \quad \\ \theta = \arccos \frac {tr(R)-1}{2} \\ \quad \\ Rn=n,(R-I)n=0,||n||_2=1 tr(R)=3cosθ+(1−cosθ)tr(nnT)=3cosθ+(1−cosθ)∣∣n∣∣22=3cosθ+(1−cosθ)=1+2cosθθ=arccos2tr(R)−1Rn=n,(R−I)n=0,∣∣n∣∣2=1 即通过旋转矩阵的迹求旋转角度，通过旋转矩阵的特征值求单位旋转向量。
轴角的表示也有奇异性，比如转30度与转390度的轴角表示是一样的。
旋转矩阵与轴角对于三维空间的旋转而言，都不够直观。
欧拉角有一种简单粗暴的旋转表示法，直接把旋转表示为围绕三个坐标轴的三个旋转，即欧拉角：
(
θ
x
,
θ
y
,
θ
z
)
(\theta_x,\theta_y,\theta_z)
(θx,θy,θz) 无人机系统中常用的欧拉角是yaw-pitch-roll，可记为偏航-俯仰-滚转角，如下图所示。 
欧拉角对于旋转而言非常直观，但是有著名的万向锁问题，导致丢失一个自由度，在计算中容易出现奇异问题。
类似于复数，四元数由一个实部与三个虚部构成： q = q 0 + q 1 i + q 2 j + q 3 k { i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j i 2 = − 1 , j 2 = − 1 , k 2 = − 1 q=q_0+q_1i+q_2j+q_3k \\ \quad \\ \begin{cases} ij=k,ji=-k \\ jk=i,kj=-i \\ ki=j,ik=-j \\ i^2=-1,j^2=-1,k^2=-1 \\ \end{cases} q=q0+q1i+q2j+q3k⎩⎪⎪⎪⎨⎪⎪⎪⎧ij=k,ji=−kjk=i,kj=−iki=j,ik=−ji2=−1,j2=−1,k2=−1 可以使用向量形式来简化四元数的表示： q = ( q 0 , q 1 , q 2 , q 3 ) = ( s , v ⃗ ) q=(q_0,q_1,q_2,q_3)=(s,\vec v) q=(q0,q1,q2,q3)=(s,v )
四元数的基本运算： q a ± q b = ( s a ± s b v ⃗ a ± v ⃗ b ) q a q b = ( s a s b − v ⃗ a ⋅ v ⃗ b , s a v ⃗ b + s b v ⃗ a + v ⃗ a × v ⃗ b ) q ∗ = ( s , − v ⃗ ) , q q ∗ = ( s 2 + v ⃗ ⋅ v ⃗ , 0 ⃗ ) ∣ ∣ q ∣ ∣ = ∣ ∣ ( s , v ⃗ ) ∣ ∣ 2 , ∣ ∣ q a q b ∣ ∣ = ∣ ∣ q a ∣ ∣ ∣ q b ∣ ∣ q − 1 = q ∗ ∣ ∣ q ∣ ∣ 2 , q q − 1 = 1 , ( q a q b ) − 1 = q b − 1 q a − 1 q_a\pm q_b = (s_a\pm s_b\,\vec v_a\pm \vec v_b) \\ \quad \\ q_aq_b = (s_as_b-\vec v_a\cdot \vec v_b,s_a\vec v_b+s_b\vec v_a+\vec v_a\times \vec v_b) \\ \quad \\ q^* = (s, -\vec v),qq^* = (s^2+\vec v \cdot \vec v,\vec 0) \\ \quad \\ ||q|| = ||(s,\vec v)||_2,||q_aq_b||=||q_a|||q_b|| \\ \quad \\ q^{-1}=\frac {q^*}{||q||^2},qq^{-1}=1,(q_aq_b)^{-1}=q_b^{-1}q_a^{-1} qa±qb=(sa±sbv a±v b)qaqb=(sasb−v a⋅v b,sav b+sbv a+v a×v b)q∗=(s,−v ),qq∗=(s2+v ⋅v ,0 )∣∣q∣∣=∣∣(s,v )∣∣2,∣∣qaqb∣∣=∣∣qa∣∣∣qb∣∣q−1=∣∣q∣∣2q∗,qq−1=1,(qaqb)−1=qb−1qa−1
单位四元数表示旋转单位四元数可以表示旋转。单位四元数实际上具有三个自由度，对应于三维空间中的旋转。
三维空间中，绕单位向量 n n n逆时针旋转 θ \theta θ，对应于单位四元数： q = ( cos θ 2 , n ⃗ sin θ 2 ) q=(\cos \frac{\theta}{2},\vec n\sin \frac{\theta}{2}) q=(cos2θ,n sin2θ) 对于向量 p = ( a , b , c ) p=(a,b,c) p=(a,b,c)而言，其旋转可表示为： q p = ( 0 , p ⃗ ) q p ′ = q q p q ∗ = ( 0 , p ⃗ ′ ) q_p=(0,\vec p) \\ q_p' = qq_pq^*=(0,\vec p') qp=(0,p )qp′=qqpq∗=(0,p ′) 在这里证明一下单位四元数旋转计算得到的四元数的实部为0： q q p q ∗ = ( cos θ 2 , n ⃗ sin θ 2 ) ( 0 , p ⃗ ) ( cos θ 2 , − n ⃗ sin θ 2 ) = ( 0 − ( n ⃗ sin θ 2 ) ⋅ p ⃗ , p ⃗ cos θ 2 + 0 + n ⃗ × p ⃗ sin θ 2 ) ( cos θ 2 , − n ⃗ sin θ 2 ) = ( − n ⃗ ⋅ p ⃗ sin θ 2 , p ⃗ cos θ 2 + n ⃗ × p ⃗ sin θ 2 ) ( cos θ 2 , − n ⃗ sin θ 2 ) = ( − n ⃗ ⋅ p ⃗ sin θ 2 cos θ 2 − ( p ⃗ cos θ 2 + n ⃗ × p ⃗ sin θ 2 ) ⋅ ( − n ⃗ sin θ 2 ) , … … ) = ( − n ⃗ ⋅ p ⃗ sin θ 2 cos θ 2 + p ⃗ ⋅ n ⃗ cos θ 2 sin θ 2 + n ⃗ × p ⃗ ⋅ n ⃗ sin θ 2 sin θ 2 , … … ) = ( n ⃗ × p ⃗ ⋅ n ⃗ sin θ 2 sin θ 2 , … … ) = ( 0 , p ⃗ ′ ) qq_pq^*=(\cos \frac{\theta}{2},\vec n\sin \frac{\theta}{2})(0,\vec p)(\cos \frac{\theta}{2},-\vec n\sin \frac{\theta}{2}) \\ \quad \\ = (0-(\vec n \sin \frac{\theta}{2})\cdot \vec p, \vec p \cos \frac{\theta}{2}+0+\vec n\times \vec p \sin \frac{\theta}{2})(\cos \frac{\theta}{2},-\vec n\sin \frac{\theta}{2}) \\ \quad \\ = (-\vec n\cdot \vec p \sin \frac{\theta}{2}, \vec p \cos \frac{\theta}{2}+\vec n\times \vec p \sin \frac{\theta}{2})(\cos \frac{\theta}{2},-\vec n\sin \frac{\theta}{2}) \\ \quad \\ = (-\vec n\cdot \vec p \sin \frac{\theta}{2}\cos \frac{\theta}{2} -(\vec p \cos \frac{\theta}{2}+\vec n\times \vec p \sin \frac{\theta}{2})\cdot (-\vec n\sin \frac{\theta}{2}),\dots \dots) \\ \quad \\ =(-\vec n\cdot \vec p \sin \frac{\theta}{2}\cos \frac{\theta}{2} +\vec p \cdot \vec n \cos \frac{\theta}{2}\sin \frac{\theta}{2}+\vec n\times \vec p \cdot \vec n \sin \frac{\theta}{2} \sin \frac{\theta}{2},\dots \dots) \\ \quad \\ = (\vec n\times \vec p \cdot \vec n \sin \frac{\theta}{2} \sin \frac{\theta}{2},\dots \dots) \\ \quad \\ = (0,\vec p') qqpq∗=(cos2θ,n sin2θ)(0,p )(cos2θ,−n sin2θ)=(0−(n sin2θ)⋅p ,p cos2θ+0+n ×p sin2θ)(cos2θ,−n sin2θ)=(−n ⋅p sin2θ,p cos2θ+n ×p sin2θ)(cos2θ,−n sin2θ)=(−n ⋅p sin2θcos2θ−(p cos2θ+n ×p sin2θ)⋅(−n sin2θ),……)=(−n ⋅p sin2θcos2θ+p ⋅n cos2θsin2θ+n ×p ⋅n sin2θsin2θ,……)=(n ×p ⋅n sin2θsin2θ,……)=(0,p ′) 最后一步中， n ⃗ × p ⃗ ⋅ n ⃗ \vec n\times \vec p \cdot \vec n n ×p ⋅n 表示 n ⃗ , p ⃗ \vec n,\vec p n ,p 的外积与 n ⃗ \vec n n 做内积。外积结果垂直于 n ⃗ , p ⃗ \vec n,\vec p n ,p ，因此再与 n ⃗ \vec n n 做内积时结果为0。
后记本篇记录了十四讲中最基础的刚体旋转和变换部分，下一篇将记录相机的成像模型。
