视觉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$为一点坐标$(x,y,z)$。

旋转与平移的结合

旋转与平移的结合可以描述为如下形式
$$a^{\prime }=R^{T}a+t$$

性质

$$R^T=R^{?1}$$
$$R{R}^T=I,det(R)=1$$

旋转矩阵与变换矩阵

$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]?T\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]$$
关于变换矩阵，其具有特殊的结构：左上角位旋转矩阵，右侧为平移向量，左下角位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=[R0T?t1?]∈R4×4∣R∈SO(3),t∈R3}

性质

求解该矩阵的逆表示一个反向的变换
$$T^{?1}=\left[\begin{array}{cc}{R}^T& ?R^{T}t\\ 0^T& 1\end{array}\right]$$

旋转向量与欧拉角

旋转矩阵的缺点

• $SO(3)$用9个量表示3个自由度上的变换，$SE(3)$用16个量表示了6个自由度上的变换，存在冗余。
• 旋转矩阵自带约束：必须是个正交矩阵且行列式为1，约束会使得求解更加困难。

因此，使用旋转轴+旋转角来刻画旋转，该向量称之为旋转向量。

旋转向量到旋转矩阵

罗德里格斯公式
$$R=cos\theta +(1?cos\theta )n{n}^T+sin\theta n^{\wedge }$$
$\wedge$为向量到反对称的转换符（笔者也称之为矩阵化，反之解矩阵），其定位如下所示
$$a^{\wedge }=\left[\begin{array}{ccc}0& ?a_3& a_2\\ a_3& 0& ?a_1\\ ?a_2& a_1& 0\end{array}\right]$$
其中$n$为旋转轴。

旋转矩阵到旋转向量

$$\theta =arccos(\frac{tr(R)?1}{2})$$

旋转轴的性质

1、位于旋转轴上的向量在旋转后不发生改变，即
$$Rn=n$$
2、旋转轴$n$是矩阵$R$特征值$1$对应的特征向量

四元数（Quaternion）

定义

$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$

物理含义

假设某个旋转是围绕单位向量$n=[n_x,n_y,n_z{]}^T$进行了角度为$\theta$的旋转，这个旋转的四元数形式为
$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}]$$
反之，亦可以从单位四元数中计算出对应的旋转轴与夹角
$$\theta =2arccos(q_0)$$
$$[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin(\theta /2)$$

运算

四元数的定义可以写为如下形式
$$q=s+xi+yj+zk=[s,v]$$
$$v=[xi,yj,zk]$$

加法

$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b]$$

乘法

$$\begin{array}{rl}{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\end{array}$$
或者
$$q_a{q}_b=[s_a{s}_b?v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]$$
矩阵外积的性质如下所是

模长

$$||q_a||=\sqrt{(s_a^2+x_a^2+y_a^2+z_a^2)}$$
$$||q_a{q}_b||=||q_a||||q_b||$$

逆

$$q^{?1}=q^{?}/||q|{|}^2$$
$$q{q}^{?1}=q^{?1}q=1$$
$$(q_a{q}_b{)}^{?1}=q_b^{?1}{q}_a^{?1}$$

数乘与点乘

$$q_a?q_b=s_a{s}_b+x_a{x}_b+y_a{y}_b+z_a{z}_b$$
$$kq=[ks,kv]$$

四元数表示旋转

假设一空间三维点$P=[x,y,z]\in {\mathbb{R}}^3$，由轴角$n,\theta$表示旋转
三维空间点用一个虚四元数表示为
$$p=[0,x,y,z]$$
用四元数表示旋转为
$$q=[cos\frac{\theta }{2},nsin\frac{\theta }{2}]$$
乘积可以表示为
$$p^{\prime }=qp{q}^{?1}$$

四元数转旋转矩阵

$$R=\left[\begin{array}{ccc}1?2q_2^2?2q_3^2& 2q_1{q}_2?2q_0{q}_3& 2q_1{q}_3+2q_0{q}_2\\ 2q_1{q}_2+2q_0{q}_3& 1?2q_1^2?2q_3^2& 2q_2{q}_3?2q_0{q}_1\\ 2q_1{q}_3?2q_0{q}_2& 2q_2{q}_3+2q_0{q}_1& 1?2q_1^2?2q_2^2\end{array}\right]$$