【SLAM】三维空间刚体运动（世界坐标系到相机坐标系）
参考《视觉slam十四讲》
旋转矩阵
三维空间向量可以表示如下
a ⃗ = [ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] \vec{a}=\left[ \begin{array}{ccc} e_1 & e_2 & e_3 \end{array} \right] \left[ \begin{array}{c} a_1 \\ a_2 \\ a_3 \end{array} \right] a=[e1e2e3]⎣⎡a1a2a3⎦⎤
外积公式为
a ⋅ b = a T ⋅ b a \cdot b=a^T \cdot b a⋅b=aT⋅b
内积公式为
a × b = [ i j k a 1 a 2 a 3 b 1 b 2 b 3 ] = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ^ b a \times b= \left[ \begin{array}{c} i&j&k \\ a_1&a_2&a_3 \\ b_1&b_2&b_3 \end{array} \right]=\left[ \begin{array}{c} a_2b_3 - a_3b_2 \\ a_3b_1 - a_1b_3 \\ a_1b_2 - a_2b_1 \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]b=\hat{a} b a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a^b
（"^"表示反对称符号）
刚体运动保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换
欧氏变换由一个旋转和一个平移两部分组成
先考虑旋转。
设某个单位正交基 [ e 1 , e 2 , e 3 ] [e_1,e_2,e_3] [e1,e2,e3]经过一次旋转，变成了 [ e 1 ′ , e 2 ′ , e 3 ′ ] [e_1^{'} ,e_2^{'},e_3^{'}] [e1′,e2′,e3′]
对于同一个向量 a，它由 [ a 1 , a 2 , a 3 ] T [a_1,a_2,a_3]^T [a1,a2,a3]T变为 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1^{'} ,a_2^{'},a_3^{'}]^T [a1′,a2′,a3′]T
则有：
[ e 1 e 2 e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ e 2 ′ e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] \left[ \begin{array}{ccc} e_1 & e_2 & e_3 \end{array} \right] \left[ \begin{array}{c} a_1 \\ a_2 \\ a_3 \end{array} \right]=\left[ \begin{array}{ccc} e_1^{'} & e_2^{'} & e_3^{'} \end{array} \right] \left[ \begin{array}{c} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{array} \right] [e1e2e3]⎣⎡a1a2a3⎦⎤=[e1′e2′e3′]⎣⎡a1′a2′a3′⎦⎤
解得：
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 2 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = R a ′ \left[ \begin{array}{c} a_1 \\ a_2 \\ a_3 \end{array} \right]=\left[ \begin{array}{c} e_1^{T}e_1^{'} & e_1^{T}e_2^{'} & e_1^{T}e_3^{'} \\ e_2^{T}e_1^{'} & e_2^{T}e_2^{'} & e_2^{T}e_3^{'} \\ e_3^{T}e_1^{'} & e_3^{T}e_2^{'} & e_3^{T}e_3^{'} \end{array} \right]\left[ \begin{array}{c} a_1^{'} \\ a_2^{'} \\ a_3^{'} \end{array} \right]=Ra^{'} ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′
矩阵 R 描述了旋转，它又称为旋转矩阵
R一个行列式为 1 的正交矩阵。反之，行列式为 1 的正交矩阵也是一个旋转矩阵
由于旋转矩阵为正交阵，它的逆（即转置）描述了一个相反的旋转
a ′ = R − 1 a = R T a a^{'}=R^{-1}a=R^{T}a a′=R−1a=RTa
在欧氏变换中，除了旋转之外还有一个平移
a ′ = R a + t a^{'}=Ra+t a′=Ra+t
假设我们进行了两次变换： R 1 , t 1 和 R 2 , t 2 R_1, t_1 和 R_2, t_2 R1,t1和R2,t2
b = R 1 a + t 1 b=R_1a+t_1 b=R1a+t1
c = R 2 b + t 2 c=R_2b+t_2 c=R2b+t2
那么从 a a a到 c c c
c = R 2 ( R 1 a + t 1 ) + t 2 c=R_2(R_1a+t_1)+t_2 c=R2(R1a+t1)+t2
[ a ′ 1 ] = [ R t o T 1 ] [ a 1 ] = T [ a 1 ] \left[ \begin{array}{cc} a^{'} \\ 1 \end{array} \right]=\left[ \begin{array}{ccc} R & t \\ o^T & 1 \end{array} \right]\left[ \begin{array}{cc} a \\ 1 \end{array} \right]=T\left[ \begin{array}{cc} a \\ 1 \end{array} \right] [a′1]=[RoTt1][a1]=T[a1]
b ‾ = T 1 a ‾ \overline{b}=T_1\overline{a} b=T1a
c ‾ = T 2 b ‾ \overline{c}=T_2\overline{b} c=T2b
c ‾ = T 1 T 2 a ‾ \overline{c}=T_1T_2\overline{a} c=T1T2a