视觉SLAM十四讲|【一】三维空间刚体运动
视觉SLAM十四讲|【一】三维空间刚体运动
旋转矩阵
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{ R \in \mathbb{R^{n \times n}} | RR^T=I,det(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
SO(n)为特殊正交群
设 a a a为一点坐标 ( x , y , z ) (x,y,z) (x,y,z)。
旋转与平移的结合
旋转与平移的结合可以描述为如下形式
a ′ = R T a + t a'=R^Ta+t a′=RTa+t
性质
R T = R − 1 R^T = R^{-1} RT=R−1
R R T = I , d e t ( R ) = 1 RR^T=I,det(R)=1 RRT=I,det(R)=1
旋转矩阵与变换矩阵
[ a ′ 1 ] = [ R t 0 T 1 ] ≜ T [ a ′ 1 ] \begin{bmatrix} a' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \triangleq T \begin{bmatrix} a' \\ 1 \end{bmatrix} [a′1]=[R0Tt1]≜T[a′1]
关于变换矩阵，其具有特殊的结构：左上角位旋转矩阵，右侧为平移向量，左下角位0向量，右下角位1，该矩阵又称之为特殊欧式群
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)={ \{ T=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R^{4 \times 4 } } | R \in SO(3), t \in \mathbb{R^3} \}} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
性质
求解该矩阵的逆表示一个反向的变换
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]
旋转向量与欧拉角
旋转矩阵的缺点
- S O ( 3 ) SO(3) SO(3)用9个量表示3个自由度上的变换， S E ( 3 ) SE(3) SE(3)用16个量表示了6个自由度上的变换，存在冗余。
- 旋转矩阵自带约束：必须是个正交矩阵且行列式为1，约束会使得求解更加困难。
因此，使用旋转轴+旋转角来刻画旋转，该向量称之为旋转向量。
旋转向量到旋转矩阵
罗德里格斯公式
R = c o s θ + ( 1 − c o s θ ) n n T + s i n θ n ∧ R = cos\theta + (1-cos\theta) n n^T + sin\theta n^{\land} R=cosθ+(1−cosθ)nnT+sinθn∧
∧ \land ∧为向量到反对称的转换符（笔者也称之为矩阵化，反之解矩阵），其定位如下所示
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a^{\land} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} a∧= 0a3−a2−a30a1a2−a10
其中 n n n为旋转轴。
旋转矩阵到旋转向量
θ = a r c c o s ( t r ( R ) − 1 2 ) \theta = arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)−1)
旋转轴的性质
1、位于旋转轴上的向量在旋转后不发生改变，即
R n = n Rn=n Rn=n
2、旋转轴 n n n是矩阵 R R R特征值 1 1 1对应的特征向量
四元数（Quaternion）
定义
q = q 0 + q 1 i + q 2 j + q 3 k q = q_0 + q_1 i + q_2 j + q_3 k q=q0+q1i+q2j+q3k
物理含义
假设某个旋转是围绕单位向量 n = [ n x , n y , n z ] T n=[n_x, n_y, n_z]^T n=[nx,ny,nz]T进行了角度为 θ \theta θ的旋转，这个旋转的四元数形式为
q = [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ] q = [cos\frac{\theta}{2}, n_x sin\frac{\theta}{2}, n_y sin\frac{\theta}{2}, n_z sin\frac{\theta}{2}] q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]
反之，亦可以从单位四元数中计算出对应的旋转轴与夹角
θ = 2 a r c c o s ( q 0 ) \theta = 2arccos(q_0) θ=2arccos(q0)
[ n x , n y , n z ] T = [ q 1 , q 2 , q 3 ] T / s i n ( θ / 2 ) [n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T / sin(\theta/2) [nx,ny,nz]T=[q1,q2,q3]T/sin(θ/2)
运算
四元数的定义可以写为如下形式
q = s + x i + y j + z k = [ s , v ] q = s + x i + y j + z k = [s, v] q=s+xi+yj+zk=[s,v]
v = [ x i , y j , z k ] v = [xi, yj, zk] v=[xi,yj,zk]
加法
q a ± q b = [ s a ± s b , v a ± v b ] q_a \pm q_b = [s_a \pm s_b, v_a \pm v_b] qa±qb=[sa±sb,va±vb]
乘法
q a q b = s a s b − x a x b − y a y b − z a z b + ( s a x b + x a s b + y a z b − z a y b ) i + ( s a y b − x a z b + y a s b + z a x b ) j + ( s a z b + x a y b − y a x b + z a s b ) k \begin{split} q_a q_b &= s_a s_b - x_a x_b - y_a y_b - z_a z_b \\ & + (s_a x_b + x_a s_b + y_a z_b -z_ay_b)i \\ & + (s_ay_b - x_az_b + y_as_b + z_ax_b)j \\ & + (s_az_b + x_ay_b - y_ax_b + z_as_b)k \end{split} qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k
或者
q a q b = [ s a s b − v a T v b , s a v b + s b v a + v a × v b ] q_a q_b = [s_as_b-v_a^Tv_b, s_av_b + s_bv_a + v_a \times v_b] qaqb=[sasb−vaTvb,savb+sbva+va×vb]
矩阵外积的性质如下所是
模长
∣ ∣ q a ∣ ∣ = ( s a 2 + x a 2 + y a 2 + z a 2 ) ||q_a||= \sqrt{(s_a^2+x_a^2+y_a^2+z_a^2)} ∣∣qa∣∣=(sa2+xa2+ya2+za2)
∣ ∣ q a q b ∣ ∣ = ∣ ∣ q a ∣ ∣ ∣ ∣ q b ∣ ∣ ||q_a q_b|| = ||q_a|| ||q_b|| ∣∣qaqb∣∣=∣∣qa∣∣∣∣qb∣∣
逆
q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 q^{-1} = q^* / ||q||^2 q−1=q∗/∣∣q∣∣2
q q − 1 = q − 1 q = 1 qq^{-1}=q^{-1}q=1 qq−1=q−1q=1
( q a q b ) − 1 = q b − 1 q a − 1 (q_a q_b)^{-1} = q_b^{-1}q_a^{-1} (qaqb)−1=qb−1qa−1
数乘与点乘
q a ⋅ q b = s a s b + x a x b + y a y b + z a z b q_a \cdot q_b = s_a s_b + x_ax_b + y_ay_b + z_az_b qa⋅qb=sasb+xaxb+yayb+zazb
k q = [ k s , k v ] kq = [ks, kv] kq=[ks,kv]
四元数表示旋转
假设一空间三维点 P = [ x , y , z ] ∈ R 3 P=[x,y,z] \in \mathbb{R^3} P=[x,y,z]∈R3，由轴角 n , θ n,\theta n,θ表示旋转
三维空间点用一个虚四元数表示为
p = [ 0 , x , y , z ] p = [0,x,y,z] p=[0,x,y,z]
用四元数表示旋转为
q = [ c o s θ 2 , n s i n θ 2 ] q=[cos\frac{\theta}{2}, nsin\frac{\theta}{2}] q=[cos2θ,nsin2θ]
乘积可以表示为
p ′ = q p q − 1 p'=qpq^{-1} p′=qpq−1
四元数转旋转矩阵
R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2 \\ 2q_1q_2+2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_0q_1 \\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix} R= 1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22