• 《视觉SLAM十四讲》公式推导(一)


    CH3 三维空间刚体运动

    CH3-1 旋转矩阵的推导

    (1)二维空间中的旋转矩阵

    易得

    { x ′ = ∣ O P ′ ∣ c o s ( θ + β ) = ∣ O P ∣ ( c o s θ ⋅ c o s β − s i n θ ⋅ s i n β ) = x c o s β − y s i n β y ′ = ∣ O P ′ ∣ s i n ( θ + β ) = ∣ O P ∣ ( s i n θ ⋅ c o s β + c o s θ ⋅ s i n β ) = y c o s β + x s i n β \left\{ x=|OP|cos(θ+β)=|OP|(cosθcosβsinθsinβ)=xcosβysinβy=|OP|sin(θ+β)=|OP|(sinθcosβ+cosθsinβ)=ycosβ+xsinβ \right. {x=OPcos(θ+β)=OP(cosθcosβsinθsinβ)=xcosβysinβy=OPsin(θ+β)=OP(sinθcosβ+cosθsinβ)=ycosβ+xsinβ

    写为矩阵形式即

    [ x ′ y ′ ] = [ c o s β − s i n β s i n β c o s β ] [ x y ] \left[xy\right]= \left[cosβsinβsinβcosβ\right] \left[xy\right] [xy]=[cosβsinβsinβcosβ][xy]

    (2)三维空间中的旋转矩阵

    以绕 z z z 轴旋转为例:

    将向量投影到 X-Y 平面,类似的有

    { x ′ = x c o s β − y s i n β y ′ = y c o s β + x s i n β z ′ = z (3-1-1) \left\{ x=xcosβysinβy=ycosβ+xsinβz=z \right. \tag{3-1-1} x=xcosβysinβy=ycosβ+xsinβz=z(3-1-1)

    写成矩阵形式

    [ x ′ y ′ z ′ ] = [ c o s β − s i n β 0 s i n β c o s β 0 0 0 1 ] [ x y z ] (3-1-2) \left[xyz\right]= \left[cosβsinβ0sinβcosβ0001\right] \left[xyz\right] \tag{3-1-2} xyz = cosβsinβ0sinβcosβ0001 xyz (3-1-2)

    记为

    a ′ = R a (3-1-3) \boldsymbol{a^{\prime}}=\boldsymbol{Ra} \tag{3-1-3} a=Ra(3-1-3)

    将式中 β \beta β 改为 − β -\beta β,来表示向量 OP’ 顺时针旋转 − β -\beta β 角度后,回到 OP 的过程,此时

    [ x y z ] = [ c o s β s i n β 0 − s i n β c o s β 0 0 0 1 ] [ x ′ y ′ z ′ ] (3-1-4) \left[xyz\right]= \left[cosβsinβ0sinβcosβ0001\right] \left[xyz\right] \tag{3-1-4} xyz = cosβsinβ0sinβcosβ0001 xyz (3-1-4)

    可以看出

    a = R T a ′ (3-1-5) \boldsymbol{a}=\boldsymbol{R^Ta'} \tag{3-1-5} a=RTa(3-1-5)

    由于 R R R 是正交矩阵,也即

    a = R − 1 a ′ (3-1-6) \boldsymbol{a}=\boldsymbol{R^{-1}a'} \tag{3-1-6} a=R1a(3-1-6)

    也就是说, R − 1 \boldsymbol{R^{-1}} R1 表示向量经反方向旋转回到原向量的过程。

    当然,也可以单纯从数学角度推导,将式(3-1-3) 两端分别左乘 R − 1 \boldsymbol{R^{-1}} R1,同样可以得到式 (3-1-5)。

    CH3-2 旋转矩阵是正交矩阵的证明

    (1)只需证明 R R T = I \boldsymbol{RR^T=I} RRT=I 即证明 R T = R − 1 \boldsymbol{R^T = R^{-1}} RT=R1 即可。

    (2)分块矩阵转置:先对整体进行转置,再对每一个分块进行转置,例如向量 A = [ A 1 , A 2 , A 3 , . . . , A N ] \boldsymbol{A=[A_1, A_2, A_3,...,A_N]} A=[A1,A2,A3,...,AN],则其转置为

    A T = [ A 1 T A 2 T A 3 T . . . A N T ] (3-2-1) \boldsymbol{A}^T=\left[\right] \tag{3-2-1} AT= A1TA2TA3T...ANT (3-2-1)

    (3) ( A B ) T = B T A T \boldsymbol{(AB)^T=B^TA^T} (AB)T=BTAT

    (4)下面证明旋转矩阵 R \boldsymbol{R} R 是正交矩阵:

    不妨设变换前后坐标系基底分别为 [ e 1 , e 2 , e 3 ] [\boldsymbol{e_1}, \boldsymbol{e_2}, \boldsymbol{e_3}] [e1,e2,e3] [ e 1 ′ , e 2 ′ , e 3 ′ ] [\boldsymbol{e_1'}, \boldsymbol{e_2'}, \boldsymbol{e_3'}] [e1,e2,e3],由于向量本身并未改变,则有

    [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] (3-2-2) \left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\right] \tag{3-2-2} [e1,e2,e3] a1a2a3 =[e1,e2,e3] a1a2a3 (3-2-2)

    将上式两端分别左乘 [ e 1 T e 2 T e 3 T ] \left[\right] e1Te2Te3T 得到,

    [ 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 ′ ] (3-2-3) \left[\right]=\left[\right]\left[\right] \tag{3-2-3} a1a2a3 = e1Te1e2Te1e3Te1e1Te2e2Te2e3Te2e1Te3e2Te3e3Te3 a1a2a3 (3-2-3)

    R = [ 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 ′ ] (3-2-4) \boldsymbol{R}=\left[\right] \tag{3-2-4} R= e1Te1e2Te1e3Te1e1Te2e2Te2e3Te2e1Te3e2Te3e3Te3 (3-2-4)

    这是向量 a ′ \boldsymbol{a'} a 经旋转得到 a \boldsymbol{a} a 的旋转矩阵。

    再将式 (3-8) 两端分别左乘 [ e 1 ′ T e 2 ′ T e 3 ′ T ] \left[\right] e1Te2Te3T 得到,

    [ 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 ] = [ a 1 ′ a 2 ′ a 3 ′ ] (3-2-5) \left[\right]\left[\right]= \left[\right] \tag{3-2-5} e1Te1e2Te1e3Te1e1Te2e2Te2e3Te2e1Te3e2Te3e3Te3 a1a2a3 = a1a2a3 (3-2-5)

    显然,这是向量 a \boldsymbol{a} a 经过反方向旋转得到 a ′ \boldsymbol{a'} a 的过程,那么有

    R − 1 = [ 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 ] (3-2-6) \boldsymbol{R^{-1}}=\left[\right] \tag{3-2-6} R1= e1Te1e2Te1e3Te1e1Te2e2Te2e3Te2e1Te3e2Te3e3Te3 (3-2-6)

    根据分块矩阵转置规则,易得

    R T = R − 1 (3-2-7) \boldsymbol{R^T = R^{-1}} \tag{3-2-7} RT=R1(3-2-7)

    证毕。

    CH3-3 变换矩阵的逆的推导

    (1)初等行变换法求矩阵的逆:将矩阵 A \boldsymbol{A} A 写成增广矩阵的形式即 ( A ∣ I ) \boldsymbol{(A | I)} (AI) ,经初等行变换,变为 ( I ∣ A − 1 ) \boldsymbol{(I | A^{-1})} (IA1),则求出矩阵 A \boldsymbol{A} A 的逆。

    (2)已知三维空间变换矩阵为:

    T = [ R 3 × 3 t 3 × 1 0 T 1 ] (3-3-1) \boldsymbol{T}=\left[\right] \tag{3-3-1} T=[R3×30Tt3×11](3-3-1)

    写成增广矩阵

    [ R 3 × 3 t 3 × 1 E 3 × 3 0 0 T 1 0 1 ] (3-3-2) \left[\right] \tag{3-3-2} [R3×30Tt3×11E3×3001](3-3-2)

    经过初等行变换,将前两列变为单位矩阵时,后两列即为 T − 1 \boldsymbol{T^{-1}} T1

    首先,将第一行左乘 R 3 × 3 − 1 \boldsymbol{R}_{3\times3}^{-1} R3×31,上式变为

    [ E 3 × 3 R 3 × 3 − 1 t 3 × 1 R 3 × 3 − 1 0 3 × 1 0 T 1 0 1 ] (3-3-3) \left[\right] \tag{3-3-3} [E3×30TR3×31t3×11R3×31003×11](3-3-3)

    再将第二行乘 − R 3 × 3 − 1 t 3 × 1 -\boldsymbol{R}_{3\times3}^{-1}\boldsymbol{t}_{3\times1} R3×31t3×1 ,加到第一行上,得

    [ E 3 × 3 0 3 × 1 R 3 × 3 − 1 − R 3 × 3 − 1 t 3 × 1 0 T 1 0 1 ] (3-3-4) \left[\right] \tag{3-3-4} [E3×30T03×11R3×310R3×31t3×11](3-3-4)

    又因为 R \boldsymbol{R} R 为正交矩阵,有 R − 1 = R T \boldsymbol{R^{-1}=R^T} R1=RT

    则变换矩阵的逆矩阵为

    T − 1 = [ R T − R T t 0 T 1 ] (3-3-5) \boldsymbol{T}^{-1}=\left[\right] \tag{3-3-5} T1=[RT0TRTt1](3-3-5)

    CH3-4 罗德里格斯公式推导

    (1)注意绕轴旋转的方式,(类似直线绕轴旋转形成圆锥的过程),也就是说,该直线是母线,所绕的轴是中心线。

    (2)向量点乘有交换律 a ⃗ ⋅ b ⃗ = b ⃗ ⋅ a ⃗ \vec{a} \cdot \vec{b}=\vec{b} \cdot \vec{a} a b =b a ;没有结合律,即 ( a ⃗ ⋅ b ⃗ ) ⋅ c ⃗ ≠ a ⃗ ⋅ ( b ⃗ ⋅ c ⃗ ) (\vec{a} \cdot \vec{b})\cdot\vec{c} \not=\vec{a} \cdot (\vec{b} \cdot \vec{c}) (a b )c =a (b c )。矩阵乘法有结合律即 ( A B ) C = A ( B C ) \boldsymbol{(AB)C=A(BC)} (AB)C=A(BC)

    (3)向量投影定理

    如图,有

    ∣ a ⃗ ∣ c o s θ = a ⃗ ⋅ b ⃗ ∣ b ⃗ ∣ = a ⃗ ⋅ b 0 ⃗ (3-4-1) |\vec{a}|cos\theta=\frac {\vec{a}\cdot\vec{b}}{|\vec{b}|}=\vec{a}\cdot\vec{b_0} \tag{3-4-1} a cosθ=b a b =a b0 (3-4-1)

    其中 b 0 ⃗ \vec{b_0} b0 为与向量 b ⃗ \vec{b} b 同向的单位向量。

    式(3-19)表示向量 a ⃗ \vec{a} a 在向量 b ⃗ \vec{b} b 上的投影的模长为 a ⃗ ⋅ b 0 ⃗ \vec{a}\cdot\vec{b_0} a b0 ,那么,其在 b ⃗ \vec{b} b 上的投影向量为 ( a ⃗ ⋅ b 0 ⃗ ) ⋅ b 0 ⃗ (\vec{a}\cdot\vec{b_0})\cdot\vec{b_0} (a b0 )b0

    (4)下面进行罗德里格斯公式的推导:

    如上图所示,向量 v ⃗ \vec{v} v u ⃗ \vec{u} u 旋转 θ \theta θ 得到 v ′ ⃗ \vec{v'} v ,其中 u ⃗ \vec{u} u 是单位向量

    ① 向量 v ⃗ \vec{v} v 分解: v ⃗ = v ⃗ ∥ + v ⃗ ⊥ \vec{v}=\vec{v}_{\parallel}+\vec{v}_{\perp} v =v +v

    ② 根据向量投影定理, v ⃗ ∥ = ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ \vec{v}_{\parallel}=(\vec{v}\cdot\vec{u})\cdot\vec{u} v =(v u )u

    ③ 综合 ① ② ,得

    v ⃗ ⊥ = v ⃗ − v ⃗ ∥ = v ⃗ − ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ (3-4-2) \vec{v}_{\perp}=\vec{v}-\vec{v}_{\parallel}=\vec{v}-(\vec{v}\cdot\vec{u})\cdot\vec{u} \tag{3-4-2} v =v v =v (v u )u (3-4-2)

    ④ 借助辅助向量 w ⃗ \vec{w} w ,且 w ⃗ = u ⃗ × v ⃗ ⊥ \vec{w}=\vec{u}\times\vec{v}_{\perp} w =u ×v ,即向量 w ⃗ \vec{w} w u ⃗ \vec{u} u v ⃗ ⊥ \vec{v}_{\perp} v 两两垂直。俯视图如下:

    则有

    w ⃗ = u ⃗ × v ⃗ ⊥ = u ⃗ × ( v ⃗ − v ⃗ ∥ ) = u ⃗ × v ⃗ − u ⃗ × v ⃗ ∥ = u ⃗ × v ⃗ (3-4-3) \tag{3-4-3} w =u ×v ===u ×(v v )u ×v u ×v u ×v (3-4-3)

    ⑤ 将 v ′ ⃗ ⊥ \vec{v'}_{\perp} v 投影到 v ⃗ ⊥ \vec{v}_{\perp} v 和 $\vec{w} $ 上,则有

    v ′ ⃗ ⊥ = v ⃗ v ′ + v ⃗ w ′ (3-4-4) \vec{v'}_{\perp}=\vec{v}_v'+\vec{v}_w' \tag{3-4-4} v =v v+v w(3-4-4)

    ⑥ 由上图

    ∣ v ′ ⃗ ⊥ ∣ = ∣ w ⃗ ∣ = ∣ v ⃗ ⊥ ∣ (3-4-5) |\vec{v'}_{\perp}|=|\vec{w}|=|\vec{v}_{\perp}| \tag{3-4-5} v =w =v (3-4-5)

    ∣ v ⃗ w ′ ∣ = ∣ v ′ ⃗ ⊥ ∣ s i n θ = ∣ w ⃗ ∣ s i n θ |\vec{v}_w'|=|\vec{v'}_{\perp}|sin\theta=|\vec{w}|sin\theta v w=v sinθ=w sinθ

    结合式(2-23),有

    v ⃗ w ′ = w ⃗ s i n θ (3-4-6) \vec{v}_w'=\vec{w}sin\theta \tag{3-4-6} v w=w sinθ(3-4-6)

    同理

    ∣ v ⃗ v ′ ∣ = ∣ v ⃗ ⊥ ∣ c o s θ |\vec{v}_v'|=|\vec{v}_{\perp}|cos\theta v v=v cosθ

    v ⃗ v ′ = v ⃗ ⊥ c o s θ (3-4-7) \vec{v}_v'=\vec{v}_{\perp}cos\theta \tag{3-4-7} v v=v cosθ(3-4-7)

    ⑦ 综合式(3-22)、(3-24)、(3-25),得

    v ′ ⃗ ⊥ = v ⃗ v ′ + v ⃗ w ′ = v ⃗ ⊥ c o s θ + w ⃗ s i n θ (3-4-8) \vec{v'}_{\perp}=\vec{v}_v'+\vec{v}_w'=\vec{v}_{\perp}cos\theta+\vec{w}sin\theta \tag{3-4-8} v =v v+v w=v cosθ+w sinθ(3-4-8)

    ⑧ 综上,

    v ′ ⃗ = v ⃗ ∥ + v ′ ⃗ ⊥ = ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ + v ⃗ ⊥ c o s θ + w ⃗ s i n θ = ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ + ( v ⃗ − ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ ) c o s θ + u ⃗ × v ⃗ s i n θ = ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ + s i n θ u ⃗ × v ⃗ + c o s θ v ⃗ − c o s θ ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ = c o s θ v ⃗ + ( 1 − c o s θ ) ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ + s i n θ u ⃗ × v ⃗ (3-4-9) \tag{3-4-9} v =====v +v (v u )u +v cosθ+w sinθ(v u )u +(v (v u )u )cosθ+u ×v sinθ(v u )u +sinθu ×v +cosθv cosθ(v u )u cosθv +(1cosθ)(v u )u +sinθu ×v (3-4-9)

    ⑨ 设矩阵

    u = [ u x u y u z ] , v = [ v x v y v z ] \boldsymbol{u}=\left[\right], \boldsymbol{v}=\left[\right] u= uxuyuz v= vxvyvz

    1)将向量点乘转换为矩阵乘法,(此处应用向量交换律矩阵结合律

    ( v ⃗ ⋅ u ⃗ ) ⋅ u ⃗ = u ⃗ ⋅ ( v ⃗ ⋅ u ⃗ ) = u ( u T v ) = u u T v (3-4-10) (\vec{v}\cdot\vec{u})\cdot\vec{u}=\vec{u}\cdot(\vec{v}\cdot\vec{u})=\boldsymbol{u}(\boldsymbol{u}^T\boldsymbol{v})=\boldsymbol{u}\boldsymbol{u}^T\boldsymbol{v} \tag{3-4-10} (v u )u =u (v u )=u(uTv)=uuTv(3-4-10)

    2)写成反对称矩阵形式

    u ⃗ × v ⃗ = ∣ i j k u x u y u z v x v y v z ∣ = [ u y v z − u z v y u z v x − u x v z u x v y − u y v x ] = [ 0 − u z u y u z 0 − u x − u y u x 0 ] [ v x v y v z ] = u ∧ v (3-4-11) \vec{u}\times\vec{v}=\left| \right|=\left[\right]= \left[\right]\left[\right]=\boldsymbol{u}^{\wedge}\boldsymbol{v} \tag{3-4-11} u ×v = iuxvxjuyvykuzvz = uyvzuzvyuzvxuxvzuxvyuyvx = 0uzuyuz0uxuyux0 vxvyvz =uv(3-4-11)

    ⑩ 因为 $ \boldsymbol{v’}=\boldsymbol{R}\boldsymbol{v}$

    综上所述,式(3-27)可化为

    R = c o s θ I + ( 1 − c o s θ ) u u T + s i n θ u ∧ (3-4-12) \boldsymbol{R}=cos\theta\boldsymbol{I}+(1-cos\theta)\boldsymbol{u}\boldsymbol{u}^T+sin\theta\boldsymbol{u}^{\wedge} \tag{3-4-12} R=cosθI+(1cosθ)uuT+sinθu(3-4-12)

    证毕。

    (5)旋转矩阵到旋转向量的转换

    将式(3-4-12)两边取迹,得

    t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( u u T ) + s i n θ t r ( u ∧ ) = 3 c o s θ + ( 1 − c o s θ ) + 0 = 1 + 2 c o s θ (3-4-13) \tag{3-4-13} tr(R)===cosθtr(I)+(1cosθ)tr(uuT)+sinθtr(u)3cosθ+(1cosθ)+01+2cosθ(3-4-13)

    其中, t r ( u u T ) = u x 2 + u y 2 + u z 2 = 1 tr(\boldsymbol{u}\boldsymbol{u}^T)=u_x^2+u_y^2+u_z^2=1 tr(uuT)=ux2+uy2+uz2=1(向量 u \boldsymbol{u} u是单位向量,模长为1)。

    那么,对于转角 θ \theta θ

    θ = a r c c o s ( t r ( R ) − 1 2 ) (3-4-14) \theta=arccos(\frac {tr(\boldsymbol{R})-1} {2}) \tag{3-4-14} θ=arccos(2tr(R)1)(3-4-14)

    对于转轴 u \boldsymbol{u} u 有,

    R u = u (3-4-15) \boldsymbol{Ru=u} \tag{3-4-15} Ru=u(3-4-15)

    即转轴 u \boldsymbol{u} u 是旋转矩阵 R \boldsymbol{R} R 特征值为 1 对应的特征向量。

  • 相关阅读:
    Vue3+TypeScript+Vite如何使用require动态引入类似于图片等静态资源
    0-1矩阵列互斥问题——回溯法 Python实现
    多路转接与Reactor
    【Mysql】mysql学习之旅03-DML数据库操纵语言
    LAN技术 -- MAC地址表、端口安全
    为什么CSS中的calc函数可能会不生效?
    编译工具链 之一 基本概念、组成部分、编译过程、命名规则
    杀不死的Perl
    【LeetCode与《代码随想录》】数组篇:做题笔记与总结-JavaScript版
    FreeRTOS-任务通知详解
  • 原文地址:https://blog.csdn.net/qq_44175983/article/details/133841883