变换矩阵与齐次坐标
欧式空间的旋转与平移不是线性的
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
c=R_2(R_1a+t_1)+t_2
c=R2(R1a+t1)+t2
引入齐次坐标——在三维向量的末尾添加1,成为四维向量
对于四维向量,将旋转和平移写在一个矩阵里,使关系变为线性关系,矩阵T称为变换矩阵(Transform Matrix)
b
~
=
T
1
a
~
,
c
~
=
T
2
b
~
⇒
c
~
=
T
2
T
1
a
~
\widetilde{b}=T_1\widetilde{a},\widetilde{c}=T_2\widetilde{b}\Rightarrow\widetilde{c}=T_2T_1\widetilde{a}
b
=T1a
,c
=T2b
⇒c
=T2T1a
关于变换矩阵,又称为特殊欧式群SE(3)
(Special Euclidean Group)
该矩阵的逆表示一个反向的变换
T 12 T_{12} T12——表示从2到1的变换
实践Eigen
旋转向量和欧拉角
旋转向量
旋转矩阵带有约束:必须是正交矩阵,行列式为1。变换矩阵也是如此
用一个三维向量表达旋转——任意旋转都可用一个旋转轴和一个旋转角来刻画
方向与旋转轴一致,长度等于旋转角——旋转向量(Axis-Angle)
对于变换矩阵,使用一个旋转向量和一个平移向量表达,六维
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式表明
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
R=\cos{\theta}I+(1-\cos{\theta})nn^T+\sin{\theta}n{\wedge}
R=cosθI+(1−cosθ)nnT+sinθn∧
关于转轴n
,旋转轴上的向量在旋转后不发生改变
R
n
=
n
Rn=n
Rn=n
所以转轴n为矩阵R特征值为1对应的特征向量
求解此方程,再归一化,便得到了旋转轴
欧拉角
使用3个分离的转角,将一个旋转分解成3次绕不同轴的旋转
由于分解的方式有多种,所以欧拉角也存在不同的定义
最常用的为ZYX轴的旋转,偏航-俯仰-滚转(yaw-pitch-roll)
- 绕物体的Z轴旋转,得到偏航角yaw
- 绕旋转之后的Y轴旋转,得到俯仰角pitch
- 绕旋转之后的X轴旋转,得到滚转角roll
rpy角的旋转顺序为ZYX
欧拉角的一个重大缺点是万向锁问题。
俯仰角为
±
90
°
\pm90°
±90°时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由3次旋转变为2次旋转)——奇异性问题。
因此,很少在SLAM程序中直接使用欧拉角表示姿态。
四元数
定义
四元数是紧凑的,即不像旋转矩阵具有冗余性(9个量描述3个自由度),而且没有奇异性。
p
=
(
s
,
v
)
T
,
v
=
(
q
1
,
q
2
,
q
3
)
T
p=(s,v)^T,v=(q_1,q_2,q_3)^T
p=(s,v)T,v=(q1,q2,q3)T
但四元数不够直观,运算稍复杂。
一个四元数拥有一个实部和三个虚部;也可用一个标量和一个向量来表示
- 三维旋转可由单位四元数来描述。
- 四元数的乘法为两个四元数的每一项相乘,最后相加。
- 四元数乘法通常不可交换,除非外积项为零。
- 两个四元数乘积的模即模的乘积。
- 四元数共轭与本身相乘,会得到一个实四元数,实部为模长的平方。
- 四元数和本身的逆的乘积为实四元数1
- 若为单位四元数,逆和共轭为同一个量。
四元数表示旋转
四元数表示旋转需先归一化
假设有一个三维点p和一个由单位四元数q指定的旋转
- 先把三维点用一个虚四元数描述——相当于把四元数的3个虚部与空间中的3个轴相对应。
旋转后的点
p
′
p'
p′表示:
p
′
=
q
p
q
−
1
p'=qpq^{-1}
p′=qpq−1
最后把
p
′
p'
p′的虚部取出,即得到旋转之后点的坐标,计算结果实部为0,为纯虚四元数。
四元数到其他旋转表示的变换
四元数乘法可以写成矩阵的乘法。
可将四元数映射为一个4×4的矩阵。
R
=
v
v
T
+
s
2
I
+
2
s
(
v
∧
)
+
(
v
∧
)
2
R=vv^T+s^2I+2s(v\wedge)+(v\wedge)^2
R=vvT+s2I+2s(v∧)+(v∧)2
相似、仿射、射影变换
相似变换
相似变换比欧式变换多了一个自由度,运行物体进行均匀缩放。
变换矩阵的旋转部分多了一个缩放因子,表示在对向量旋转之后,可以在x,y,z三个坐标上进行均匀缩放。
三维相似变换的集合——相似变换群,记作Sim(3)
仿射变换
仿射变换也叫做正交投影,经过仿射变换之后,立方体不再是方的,但是各个面仍然是平行四边形。
射影变换
射影变换是最一般的变换,从真实世界到相机照片的变换可以看成一个射影变换。
变换名称 | 自由度 | 不变性质 |
---|---|---|
欧式变换 | 6 | 长度、夹角、体积 |
相似变换 | 7 | 体积比 |
仿射变换 | 12 | 平行性、体积比 |
射影变换 | 15 | 接触平面的相交和相切 |
实践Eigen几何
四元数在使用之前需归一化
可视化演示
显示运动轨迹
李群与李代数
通过李群-李代数间的转换关系,将位姿估计变成无约束的优化问题,简化求解方式。
基础
三维旋转矩阵构成了特殊正交群SO(3),变换矩阵构成了特殊欧式群SE(3)。
二者对于加法不封闭,对乘法封闭
乘法对应旋转或变换的复合,两个旋转矩阵相乘表示做了两次旋转。
- 对于只有一个运算的集合,称之为群。
群
群是一种集合加上一种运算的代数结构。
G
=
(
A
,
⋅
)
G=(A,·)
G=(A,⋅)
- 封闭性: a 1 ⋅ a 2 ∈ A a_1·a_2{\in}A a1⋅a2∈A
- 结合律: ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) (a_1·a_2)·a_3=a_1·(a_2·a_3) (a1⋅a2)⋅a3=a1⋅(a2⋅a3)
- 幺元: a 0 ⋅ a = a ⋅ a 0 = a a_0·a=a·a_0=a a0⋅a=a⋅a0=a
- 逆: a − 1 ∈ A , a ⋅ a − 1 = a 0 a^{-1}{\in}A,a·a^{-1}=a_0 a−1∈A,a⋅a−1=a0
李群是指具有连续(光滑)性质的群。
SO(n)和SE(n)在实数空间上是连续的。