前言
本博客为主要学习《视觉SLAM十四讲》第3讲、《机器人学的状态估计》第6章三位几何学基础、《计算机视觉-算法和应用》第2章2.1几何基元变换等SLAM内容的总结与整理。
主要包括:
1、点、向量、坐标系、直线、平面等几何学基础
2、旋转矩阵、角轴、欧拉角、四元数等表示坐标系旋转的方法,罗德里格斯公式的证明,四元数左乘右乘、导数的推导
3、2D、3D空间的欧式、相似、仿射、射影变换性质
1、几何学基础
1.1 点
2D点,如图像中的像素坐标,可以用一对数值表示:
x
=
(
x
,
y
)
∈
R
2
x =(x,y) \in \mathbf R^2
x=(x,y)∈R2。
也可以用齐次坐标表示:
x
~
=
(
x
~
,
y
~
,
w
~
)
∈
P
2
\widetilde x = (\widetilde x,\widetilde y,\widetilde w) \in \mathbf P^2
x
=(x
,y
,w
)∈P2
其中仅在尺度上不同的矢量被视为等同的。 P 2 = R 3 − ( 0 , 0 , 0 ) \mathbf P^2=\mathbf R^3-(0,0,0) P2=R3−(0,0,0)称作2D投影空间。齐次矢量 x ~ \widetilde x x 可以通过除以最后一个元素 w ~ \widetilde w w 转换为非齐次矢量 x x x,即 x ~ = ( x ~ , y ~ , w ~ ) = w ~ ( x , y , 1 ) = w ~ x ˉ \widetilde x = (\widetilde x,\widetilde y,\widetilde w)=\widetilde w(x,y,1)=\widetilde w \bar x x =(x ,y ,w )=w (x,y,1)=w xˉ, x ˉ \bar x xˉ成为增广矢量。
3D点,也可以写成非齐次坐标 x = ( x , y , z ) ∈ R 3 x =(x,y,z) \in \mathbf R^3 x=(x,y,z)∈R3或齐次坐标 x ~ = ( x ~ , y ~ , z ~ , w ~ ) = w ~ ( x , y , z , 1 ) ∈ P 3 \widetilde x = (\widetilde x,\widetilde y,\widetilde z,\widetilde w) =\widetilde w(x,y,z,1)\in \mathbf P^3 x =(x ,y ,z ,w )=w (x,y,z,1)∈P3。
1.2 向量与坐标系
向量是线性空间中的一个元素,可以想象成从原点指向某处的一个箭头。只有当指定三维空间的坐标系时,才存在向量在此坐标系下的坐标。例如确定了三维空间中坐标系 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3),向量在这组基下的坐标为:
a = [ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = a 1 e 1 + a 2 e 2 + a 3 e 3 a=[e_1,e_2,e_3]\left[ \begin{matrix} a_1 \\a_2\\a_3 \end{matrix} \right]=a_1e_1+a_2e_2+a_3e_3 a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
坐标系通常由3个正交的坐标系组成。右手坐标系即给定x轴和y轴时,z轴的方向可以通过右手法则由 x × y x\times y x×y定义。
向量内积:
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
c
o
s
<
a
,
b
>
a \cdot b = a^Tb=\sum_{i=1}^{3}a_ib_i=|a||b|cos<a,b>
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos<a,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{matrix} i&j&k \\a_1&a_2&a_3\\b_1&b_2&b_3 \end{matrix} \right\|=\left[ \begin{matrix} a_2b_3-a_3b_2 \\a_3b_1-a_1b_3\\a_1b_2-a_2b_1 \end{matrix} \right]=\left[ \begin{matrix} 0&-a_3&a_2 \\a_3&0&-a_1\\-a_2&a_1&0 \end{matrix} \right]b \triangleq a^\land b
a×b=∥∥∥∥∥∥ia1b1ja2b2ka3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b≜a∧b
引入^符号,将a写成一个反对称矩阵,将向量的外积转变为向量与矩阵的乘法。外积只对向量存在定义。
在三维几何中,向量a和向量b的外积结果是垂直于a和b向量构成平面的法向量,大小为向量a和向量b构成的平行四边形的面积。
1.3 2D直线
2D直线也可以用齐次坐标表达
l
~
=
(
a
,
b
,
c
)
\widetilde l=(a,b,c)
l
=(a,b,c)。对应的直线方程为:
x
ˉ
l
~
=
a
x
+
b
y
+
c
=
0
\bar x \widetilde l=ax+by+c=0
xˉl
=ax+by+c=0
也可以规范化直线方程,使得
l
=
(
n
^
x
,
n
^
y
,
d
)
=
(
n
^
,
d
)
,
∣
∣
n
^
∣
∣
=
1
l=(\hat n_x,\hat n_y,d)=(\hat n,d),||\hat n||=1
l=(n^x,n^y,d)=(n^,d),∣∣n^∣∣=1
此时 n ^ \hat n n^是垂直于直线的法向量,d是其到原点的距离。但是这样有个例外,就是无穷远处的直线 l ~ = ( 0 , 0 , 1 ) \widetilde l=(0,0,1) l =(0,0,1),它包含了所有无穷远处的点。
使用齐次坐标系,可以计算两条直线的角点:
x
~
=
l
~
1
×
l
~
2
\widetilde x=\widetilde l_1 \times \widetilde l_2
x
=l
1×l
2
计算两点的直线:
l
~
=
x
~
1
×
x
~
2
\widetilde l = \widetilde x_1 \times \widetilde x_2
l
=x
1×x
2
点到直线的距离
1)对于点
(
x
0
,
y
0
)
(x_0,y_0)
(x0,y0),到直线
a
x
+
b
y
+
c
=
0
ax+by+c=0
ax+by+c=0的距离d可以如下计算:
d
=
∣
a
x
0
+
b
y
0
+
c
a
2
+
b
2
∣
d=|\frac{ax_0+by_0+c}{\sqrt{a^2+b^2}}|
d=∣a2+b2ax0+by0+c∣
2)对于点A、B、C,求点A到直线BC的距离还可以这样算:
d
=
∣
A
B
×
B
C
∣
∣
B
C
∣
d=\frac{|AB \times BC|}{|BC|}
d=∣BC∣∣AB×BC∣
1.4 3D平面
3D平面也可以表达为齐次坐标
m
^
=
(
a
,
b
,
c
,
d
)
\hat m=(a,b,c,d)
m^=(a,b,c,d),对应的平面方程为:
x
ˉ
⋅
m
^
=
a
x
+
b
y
+
c
z
+
d
=
0
\bar x \cdot \hat m=ax +by+cz+d=0
xˉ⋅m^=ax+by+cz+d=0
也可以规范化平面方程,使得
l
=
(
n
^
x
,
n
^
y
,
n
^
z
,
d
)
=
(
n
^
,
d
)
,
∣
∣
n
^
∣
∣
=
1
l=(\hat n_x,\hat n_y,\hat n_z,d)=(\hat n,d),||\hat n||=1
l=(n^x,n^y,n^z,d)=(n^,d),∣∣n^∣∣=1
此时 n ^ \hat n n^是垂直于平面的法向量,d是其到原点的距离。同样有个例外,就是无穷远处的平面 l ~ = ( 0 , 0 , 0 , 1 ) \widetilde l=(0,0,0,1) l =(0,0,0,1),它包含了所有无穷远处的点。
点到平面的距离
1)对于点
(
x
0
,
y
0
)
(x_0,y_0)
(x0,y0),到平面
a
x
+
b
y
+
c
z
+
d
=
0
ax+by+cz+d=0
ax+by+cz+d=0的距离d可以如下计算:
d
=
∣
a
x
0
+
b
y
0
+
c
z
0
+
d
a
2
+
b
2
+
c
2
∣
d=|\frac{ax_0+by_0+cz_0+d}{\sqrt{a^2+b^2+c^2}}|
d=∣a2+b2+c2ax0+by0+cz0+d∣
2)对于点A、B、C、D,求点A到平面BCD的距离还可以这样算:
d
=
∣
(
B
C
×
B
D
)
⋅
A
B
∣
∣
B
C
×
B
D
∣
d=\frac{|(BC \times BD)\cdot AB|}{|BC \times BD|}
d=∣BC×BD∣∣(BC×BD)⋅AB∣
1.5 3D直线
3D的直线表达可以用直线上的两个点
(
p
,
q
)
(p,q)
(p,q),直线上的其他点可以表示为这两个点的线性组合:
r
=
(
1
−
λ
)
p
+
λ
q
,
r
~
=
(
1
−
λ
)
p
~
+
λ
q
~
r=(1-\lambda)p+\lambda q,\widetilde r=(1-\lambda)\widetilde p+\lambda \widetilde q
r=(1−λ)p+λq,r
=(1−λ)p
+λq
一种特殊情况是当第二个点位于无穷远时,即
q
~
=
(
d
^
x
,
d
^
y
,
d
^
z
,
0
)
=
(
d
^
,
0
)
\widetilde q=(\hat d_x,\hat d_y,\hat d_z,0)=(\hat d,0)
q
=(d^x,d^y,d^z,0)=(d^,0)。这里
d
^
\hat d
d^即为直线的方向。可以讲非齐次坐标的3D直线方程重写为:
r
=
p
+
λ
q
^
r=p+\lambda \hat q
r=p+λq^
2、旋转表示方法
2.1 旋转矩阵与欧式变换
假设某个单位正交基
(
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
′
]
[e_1,e_2,e_3]\left[ \begin{matrix} a_1 \\a_2\\a_3 \end{matrix} \right]=[e'_1,e'_2,e'_3]\left[ \begin{matrix} a'_1 \\a'_2\\a'_3 \end{matrix} \right]
[e1,e2,e3]⎣⎡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{matrix} a_1 \\a_2\\a_3 \end{matrix} \right]=\left[ \begin{matrix}e_1^Te'_1 & e_1^Te'_2&e_1^Te'_3\\e_2^Te'_1 & e_2^Te'_2&e_2^Te'_3\\e_3^Te'_1 & e_3^Te'_2&e_3^Te'_3\end{matrix} \right] \left[ \begin{matrix} a'_1 \\a'_2\\a'_3 \end{matrix} \right] \triangleq Ra' ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
旋转矩阵R为行列式为1的正交矩阵,其集合定义如下:
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(n)=\{R∈R^{n\times n}|RR^T=I,det(R)=1\}
SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
SO(n)为特殊正交群(Special Orthogonal Group)。
欧拉旋转定理:刚体在三维空间里的一般运动可以分解为刚体上方某一点的平移,和绕经过此点的旋转轴的转动。
欧式变换可以使用旋转矩阵R和平移向量t完整地描述:
a
′
=
R
a
+
t
a'=Ra+t
a′=Ra+t
引入齐次坐标,重写变换矩阵为:
[
a
′
1
]
=
[
R
T
0
T
1
]
[
a
1
]
≜
T
[
a
1
]
\left[ \begin{matrix} a' \\1 \end{matrix} \right] =\left[ \begin{matrix} R&T\\0^T&1 \end{matrix} \right]\left[ \begin{matrix} a \\1 \end{matrix} \right] \triangleq T\left[ \begin{matrix} a \\1 \end{matrix} \right]
[a′1]=[R0TT1][a1]≜T[a1]
变换矩阵T又被称为特殊欧式群(Special Euclidean Group):
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3)=\{T=\left[ \begin{matrix} R&t\\0^T&1 \end{matrix} \right]\in R^{4\times 4}|R\in SO(3),t \in R^3\}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}
变换矩阵的逆为: [ R T − R T t 0 T 1 ] \left[ \begin{matrix} R^T&-R^Tt\\0^T&1 \end{matrix} \right] [RT0T−RTt1]
习惯上我们采用旋转矩阵
R
12
R_{12}
R12或者
R
2
1
R^1_2
R21表示从坐标系2到坐标系1的旋转矩阵:
a
1
=
R
12
a
2
a_1=R_{12}a_2
a1=R12a2
对SLAM来说,我们会用到相机坐标系和世界坐标系。
T
w
c
T_{wc}
Twc表示相机坐标系到世界坐标系的变换,常用来表示相机的位姿,因为更加直观:其平移部分是相机原点在世界坐标系下的坐标:
p
w
=
T
w
c
p
c
=
T
w
c
0
=
t
w
c
p_w=T_{wc} p_c=T_{wc} 0=t_{wc}
pw=Twcpc=Twc0=twc
T c w T_{cw} Tcw表示世界坐标系到相机坐标系的变换,在SLAM输出的结果中更加常用。
假设世界坐标系到相机坐标系的变换Rt,相机中心在世界坐标系中的位置:
R p w + t = p c = 0 ⇒ p w = − R T t Rp_w+t=p_c=0\Rightarrow p_w=-R^Tt Rpw+t=pc=0⇒pw=−RTt
相机朝向(Z轴)在世界坐标系下的方向为:
r
c
=
Z
c
−
O
c
a
m
c
,
Z
c
=
(
0
,
0
,
1
)
T
,
O
c
a
m
c
=
(
0
,
0
,
0
)
T
r^c=Z^c-O^c_{cam},Z^c=(0,0,1)^T,O^c_{cam}=(0,0,0)^T
rc=Zc−Ocamc,Zc=(0,0,1)T,Ocamc=(0,0,0)T
r w = ( R T Z c − R T t ) − ( R T O c a m c − R T t ) = R T ( 0 0 1 ) r^w=(R^TZ^c-R^Tt)-(R^TO^c_{cam}-R^Tt)=R^T \left( \begin{matrix} 0\\0\\1 \end{matrix} \right) rw=(RTZc−RTt)−(RTOcamc−RTt)=RT⎝⎛001⎠⎞
即旋转矩阵的第三行。
2.2 旋转向量
任意旋转可以用一个旋转轴和一个旋转角表示。定义旋转轴为单位向量
n
=
[
a
1
,
a
2
,
a
3
]
T
n=[a_1,a_2,a_3]^T
n=[a1,a2,a3]T,满足:
n
T
n
=
a
1
2
+
a
2
2
+
a
3
2
=
1
n^Tn=a^2_1+a^2_2+a^2_3=1
nTn=a12+a22+a32=1
定义旋转角为 θ \theta θ,得到三维的旋转向量 θ n \theta n θn(或角轴Angle-Axis)来描述旋转。
从旋转向量到旋转矩阵的转换有罗德里格斯公式:
R
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
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∧
罗德里格斯公式的证明:
假设初始向量
v
v
v 绕旋转轴
k
k
k(单位向量) 旋转
θ
\theta
θ 角得到
v
r
o
t
v_{rot}
vrot。
对v进行向量分解:
v
=
v
⊥
+
v
∥
v = v_{\perp} +v_{\parallel}
v=v⊥+v∥,其中
v
∥
=
(
v
⋅
k
)
k
v_{\parallel} = (v\cdot k)k
v∥=(v⋅k)k
由旋转过程平行向量不变性得: v r o t ∥ = v ∥ v_{rot\parallel}=v_{\parallel} vrot∥=v∥
而 v r o t ⊥ = c o s θ v ⊥ + ( s i n θ ∣ v ⊥ ∣ ) k × v ⊥ ∣ v ⊥ ∣ = c o s θ v ⊥ + s i n θ k × v v_{rot\perp}=cos\theta v_{\perp} + (sin\theta|v_{\perp}|) k\times \frac{v_{\perp}}{|v_{\perp}|}=cos\theta v_{\perp} + sin\theta k\times v vrot⊥=cosθv⊥+(sinθ∣v⊥∣)k×∣v⊥∣v⊥=cosθv⊥+sinθk×v
因此
v r o t = v r o t ∥ + v r o t ⊥ = v ∥ + c o s θ v ⊥ + s i n θ k × v v_{rot}=v_{rot\parallel}+v_{rot\perp}=v_{\parallel}+cos\theta v_{\perp} + sin\theta k\times v vrot=vrot∥+vrot⊥=v∥+cosθv⊥+sinθk×v
= c o s θ v + ( 1 − c o s θ ) ( v ⋅ k ) k + s i n θ k × v =cos\theta v+(1-cos\theta)(v\cdot k)k+ sin\theta k\times v =cosθv+(1−cosθ)(v⋅k)k+sinθk×v
其中
(
v
⋅
k
)
k
=
k
(
v
⋅
k
)
=
k
(
k
T
v
)
=
k
k
T
v
(v\cdot k)k=k(v\cdot k)=k(k^Tv)=kk^Tv
(v⋅k)k=k(v⋅k)=k(kTv)=kkTv
所以
v
r
o
t
=
c
o
s
θ
v
+
(
1
−
c
o
s
θ
)
k
k
T
v
+
s
i
n
θ
k
∧
v
=
R
t
v_{rot}=cos\theta v+(1-cos\theta)kk^Tv+ sin\theta k^{\land} v=Rt
vrot=cosθv+(1−cosθ)kkTv+sinθk∧v=Rt
R = c o s θ I + ( 1 − c o s θ ) k k T + s i n θ k ∧ R=cos\theta I +(1-cos\theta)kk^T+ sin\theta k^{\land} R=cosθI+(1−cosθ)kkT+sinθk∧
如果用叉乘来表示 v ⊥ v_{\perp} v⊥,有: v ⊥ = − k × ( k × v ) v_{\perp}=-k \times(k \times v) v⊥=−k×(k×v)
v r o t = v r o t ∥ + v r o t ⊥ = v ∥ + c o s θ v ⊥ + s i n θ k × v v_{rot}=v_{rot\parallel}+v_{rot\perp}=v_{\parallel}+cos\theta v_{\perp} + sin\theta k\times v vrot=vrot∥+vrot⊥=v∥+cosθv⊥+sinθk×v
= v + ( 1 − c o s θ ) k × ( k × v ) + s i n θ k × v = v+(1-cos\theta) k \times(k \times v)+sin\theta k\times v =v+(1−cosθ)k×(k×v)+sinθk×v
= ( I + ( 1 − c o s θ ) k ∧ k ∧ + s i n θ k ∧ ) v = (I+(1-cos \theta)k^{\land}k^{\land}+sin\theta k^{\land})v =(I+(1−cosθ)k∧k∧+sinθk∧)v
罗德里格斯公式也可以写成:
R = I + ( 1 − c o s θ ) k ∧ k ∧ + s i n θ k ∧ R=I +(1-cos\theta)k^{\land}k^{\land} +sin\theta k^{\land} R=I+(1−cosθ)k∧k∧+sinθk∧
同时可以得到旋转矩阵到旋转向量的变换:
两边取迹:
t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( n n T ) + s i n θ t r ( n ∧ ) = 3 c o s θ + 1 − c o s θ = 1 + 2 c o s θ tr(R)=cos\theta tr(I)+(1-cos\theta)tr(nn^T)+sin \theta tr(n^{\land})\\ =3cos\theta+1-cos\theta=1+2cos\theta tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n∧)=3cosθ+1−cosθ=1+2cosθ
因此:
θ = a r c c o s ( t r ( R ) − 1 2 ) \theta=arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)−1)
对于转轴
n
n
n,由于旋转轴上的向量在旋转后不变,有:
R
n
=
n
Rn=n
Rn=n
因此也可以说,转轴是旋转矩阵的特征值1所对应的特征向量。
2.3 欧拉角
欧拉角(Euler Angles),将旋转分解为三个方向上的转动。分解方式有多种,同时根据绕固定轴还是绕旋转后的轴旋转也会有不一样的定义方式。
比如按Z-Y-X顺序转动得到yaw(偏航角)-pitch(俯仰角)-roll(滚转角),用
[
r
,
p
,
y
]
T
[r,p,y]^T
[r,p,y]T表示
欧拉角的缺点是会遇到万向锁问题:在俯仰角为±90°时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失一个自由度。这被成为奇异性问题。因此欧拉角不适于插值和迭代,往往用于人机交互中。
理论可以证明,对于只有三个参数的表示形式,则必然会存在奇异点。所以没有完美的旋转表示形式。
2.4 四元数
四元数的相关知识推荐看《Quaternion kinematics for the error-state Kalman filter》,这里只对重要的性质做简单介绍。
四元数具有一个实部和三个虚部,常用一个标量和一个向量来表示:
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
=
[
s
,
v
]
T
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
q=q_0+q_1i+q_2j+q_3k=[s,v]^T,s=q_0\in R,v=[q_1,q_2,q_3]^T\in R^3
q=q0+q1i+q2j+q3k=[s,v]T,s=q0∈R,v=[q1,q2,q3]T∈R3
四元数的性质
这里主要讨论四元数的左右乘和求导。
四元数的乘法有:
p
⊗
q
=
[
s
p
s
q
−
v
p
T
v
q
s
p
v
q
+
s
q
v
p
+
v
p
×
v
q
]
=
[
p
]
L
q
=
[
q
]
R
p
p\otimes q=\left[ \begin{matrix} s_ps_q-v^T_pv_q\\s_pv_q+s_qv_p+v_p \times v_q \end{matrix} \right]=[p]_Lq=[q]_Rp
p⊗q=[spsq−vpTvqspvq+sqvp+vp×vq]=[p]Lq=[q]Rp
其中:
[
p
]
L
=
[
s
p
−
v
p
T
v
p
s
p
I
+
v
p
∧
]
=
s
p
I
+
[
0
−
v
p
T
v
p
v
p
∧
]
,
[
p
]
R
=
[
s
q
−
v
q
T
v
q
s
q
I
−
v
q
∧
]
=
s
q
I
+
[
0
−
v
q
T
v
q
−
v
q
∧
]
[p]_L=\left[ \begin{matrix} s_p&-v_p^T\\v_p&s_pI+v_p^{\land} \end{matrix} \right]=s_pI+\left[ \begin{matrix} 0&-v_p^T\\v_p&v_p^{\land} \end{matrix} \right],[p]_R=\left[ \begin{matrix} s_q&-v_q^T\\v_q&s_qI-v_q^{\land} \end{matrix} \right]=s_qI+\left[ \begin{matrix} 0&-v_q^T\\v_q&-v_q^{\land} \end{matrix} \right]
[p]L=[spvp−vpTspI+vp∧]=spI+[0vp−vpTvp∧],[p]R=[sqvq−vqTsqI−vq∧]=sqI+[0vq−vqT−vq∧]
注意如果写成虚部在前实部在后的四元数形式,左乘和右乘矩阵的符号有所变化。
同时有性质:
[
p
]
L
[
q
]
R
=
[
q
]
R
[
p
]
L
[p]_L[q]_R=[q]_R[p]_L
[p]L[q]R=[q]R[p]L
单位四元数满足: q T q = 1 q^Tq=1 qTq=1
四元数与旋转
假设某旋转是绕着单位向量
n
=
[
n
x
,
n
y
,
n
z
]
n=[n_x,n_y,n_z]
n=[nx,ny,nz]进行角度为
θ
\theta
θ的旋转,那么四元数可表示为:
q
=
[
c
o
s
θ
2
,
n
x
s
i
n
θ
2
,
n
y
s
i
n
θ
2
,
n
z
s
i
n
θ
2
]
T
q=[cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2}]^T
q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
反之:
θ
=
2
a
r
c
c
o
s
q
0
,
n
=
[
q
1
,
q
2
,
q
3
]
T
/
s
i
n
θ
2
\theta = 2arccos q_0,n=[q_1,q_2,q_3]^T/sin\frac{\theta}{2}
θ=2arccosq0,n=[q1,q2,q3]T/sin2θ
在四元数中,任意的旋转都可以由两个相反的四元数表示。假设空间三维点
p
=
[
x
,
y
,
z
]
T
∈
R
3
p=[x,y,z]^T\in R^3
p=[x,y,z]T∈R3经过轴角
n
θ
n\theta
nθ变为
p
′
p'
p′。首先将三维空间点用虚四元数表示:
p
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
p=[0,x,y,z]^T=[0,v]^T
p=[0,x,y,z]T=[0,v]T
用四元数表示旋转:
q
=
[
c
o
s
θ
2
,
n
s
i
n
θ
2
]
,
p
′
=
q
p
q
−
1
q=[cos\frac{\theta}{2},nsin\frac{\theta}{2}],p'=qpq^{-1}
q=[cos2θ,nsin2θ],p′=qpq−1
结果仍为纯虚四元数,虚部的三个分量表示旋转后的3D点坐标。
四元数的导数
首先需要注意的是,对于用任何方式表示旋转时,对一个旋转再进行旋转有左乘和右乘两种形式,分别代表不同的意义。
首先定义四元数的导数有:
q ˙ ≜ lim Δ t → 0 q ( t + Δ t ) − q ( t ) Δ t = lim Δ t → 0 q ⊗ Δ q L − q Δ t = lim Δ t → 0 Δ q G ⊗ q − q Δ t \dot q\triangleq \lim_{\Delta t \to0}\frac{q(t+\Delta t )-q(t)}{\Delta t }=\lim_{\Delta t \to0}\frac{q\otimes \Delta q_L-q}{\Delta t }=\lim_{\Delta t \to0}\frac{ \Delta q_G\otimes q-q}{\Delta t } q˙≜Δt→0limΔtq(t+Δt)−q(t)=Δt→0limΔtq⊗ΔqL−q=Δt→0limΔtΔqG⊗q−q
其中 Δ q L \Delta q_L ΔqL表示局部坐标系中的角度扰动,而 Δ q G \Delta q_G ΔqG表示全局(世界)坐标系中的角度扰动。我们实际测量的角速度往往是在物体坐标系下的角速度 w w w(例如VIO中通过IMU陀螺仪获得),故常采用四元数右乘形式。
测量的角速度可以定义为:
w L ( t ) ≜ d ϕ L ( t ) d t ≜ lim Δ t → 0 Δ ϕ L Δ t w_L(t)\triangleq \frac{d\phi_L(t)}{dt}\triangleq \lim_{\Delta t \to0}\frac{\Delta \phi_L}{\Delta t } wL(t)≜dtdϕL(t)≜Δt→0limΔtΔϕL
那么有:
q ˙ ≜ lim Δ t → 0 q ⊗ Δ q L − q Δ t = lim Δ t → 0 q ⊗ ( [ 1 Δ ϕ L 2 ] − [ 1 0 ] ) Δ t \dot q \triangleq \lim_{\Delta t \to0}\frac{q\otimes \Delta q_L-q}{\Delta t } = \lim_{\Delta t \to0}\frac{q\otimes (\left[ \begin{matrix} {1}\\ \frac{\Delta \phi_L}{2}\end{matrix} \right]-\left[ \begin{matrix} 1\\ 0\end{matrix} \right])}{\Delta t } q˙≜Δt→0limΔtq⊗ΔqL−q=Δt→0limΔtq⊗([12ΔϕL]−[10])
= lim Δ t → 0 q ⊗ [ 0 Δ ϕ L 2 ] Δ t = 1 2 q ⊗ [ 0 w L ] =\lim_{\Delta t \to0}\frac{q\otimes \left[ \begin{matrix} {0}\\ \frac{\Delta \phi_L}{2}\end{matrix} \right]}{\Delta t } =\frac{1}{2}q \otimes \left[ \begin{matrix} {0}\\ w_L\end{matrix} \right] =Δt→0limΔtq⊗[02ΔϕL]=21q⊗[0wL]
因此有:
q ˙ = 1 2 q ⊗ w L = 1 2 Ω ( w L ) q , Ω ( w ) ≜ [ w ] R = [ 0 − w T w − w ∧ ] \dot q=\frac{1}{2}q \otimes w_L=\frac{1}{2} \Omega(w_L)q,\Omega(w) \triangleq [w]_R=\left[ \begin{matrix} 0&-w^T\\w&-w^{\land} \end{matrix} \right] q˙=21q⊗wL=21Ω(wL)q,Ω(w)≜[w]R=[0w−wT−w∧]
如果是左乘形式,即将角速度转换到世界坐标系下,有:
q ˙ = 1 2 w G ⊗ q \dot q=\frac{1}{2}w_G \otimes q q˙=21wG⊗q
3、坐标变换性质
3.1 2D坐标变换
变换 | 矩阵 | 自由度 | 保持 | 图标 |
---|---|---|---|---|
平移 | [ I ∣ t ] 2 × 3 [I\vert t]_{2\times3} [I∣t]2×3 | 2 | 方向 | 正方形 |
欧式 | [ R ∣ t ] 2 × 3 [R\vert t]_{2\times3} [R∣t]2×3 | 3 | 长度 | 旋转的正方形 |
相似 | [ s R ∣ t ] 2 × 3 [sR\vert t]_{2\times3} [sR∣t]2×3 | 4 | 夹角 | 旋转缩放的正方形 |
仿射 | [ A ] 2 × 3 [A]_{2\times3} [A]2×3 | 6 | 平行性 | 平行四边形 |
投影 | [ H ~ ] 3 × 3 [\widetilde H]_{3\times3} [H ]3×3 | 8 | 直线性 | 四边形 |
其中 R = [ c o s θ s i n θ − s i n θ c o s θ ] R=\left[ \begin{matrix} cos\theta & sin\theta \\ -sin\theta &cos\theta \end{matrix} \right] R=[cosθ−sinθsinθcosθ]
3.2 3D坐标变换
变换名称 | 矩阵形式 | 自由度 | 不变性之 |
---|---|---|---|
平移变换 | [ I t 0 T 1 ] \left[ \begin{matrix}I & t \\0^T &1 \end{matrix} \right] [I0Tt1] | 3 | 方向 |
欧式变换 | [ R t 0 T 1 ] \left[ \begin{matrix}R & t \\0^T &1 \end{matrix} \right] [R0Tt1] | 6 | 长度、体积 |
相似变换 | [ s R t 0 T 1 ] \left[ \begin{matrix}sR & t \\0^T &1 \end{matrix} \right] [sR0Tt1] | 7 | 夹角 |
仿射变换 | [ A t 0 T 1 ] \left[ \begin{matrix}A & t \\0^T &1 \end{matrix} \right] [A0Tt1] | 12 | 平行性、体积比 |
射影变换 | [ A t a T v ] \left[ \begin{matrix}A & t \\a^T &v \end{matrix} \right] [AaTtv] | 15 | 接触平面的相交和相切 |
从世界坐标系到相机照片的变换是一个射影变换,如果相机焦距无穷远则为仿射变换。