本讲主要介绍了坐标欧式变换(旋转+平移)。其中,旋转变换不同表示方式之间转换的推导占大篇幅,多是些线性代数的知识。若对欧氏空间、反对称矩阵、基、正交矩阵等基础概念尚存有❓,不妨先回头温故相关的概念,再学习本讲内容将会事半功倍。
视觉SLAM学习打卡【2】
- 一、为什么要进行坐标变换
- 二、数学基础
- 1.向量坐标表示
- 2.内积/数量积/点积 & 外积/向量积/叉积
- 3.反对称矩阵
- 4.四元数
- 三、坐标的欧式变换
- 1.何为欧式变换
- 2.旋转矩阵
- 3.欧式变换的向量表示
- 4.变换矩阵与齐次坐标
- 四、旋转的不同表现形式
- 五、旋转不同表现形式之间的转换
- 六、常见变换的性质比较
- 七、坐标变换的例子
一、为什么要进行坐标变换
假设我们在景区,目前我们手上有两个坐标系:世界坐标系(基底1)和以眼睛为原点的“相机”坐标系(基底2)(眼睛类似于机器人的相机,二者都为传感器)。眼睛看到的物体会基于我们自身的坐标系有一个坐标x1,当我们需要判断自己的位置(定位),并去到下一个景点(导航)时,仅仅依靠眼睛收到的信息还不够,更需要知道这些物体基于世界坐标系的一个坐标x2,即需要进行坐标转换x1—>x2。
二、数学基础
1.向量坐标表示
向量a在线性空间的基[e1,e2,e3]下的坐标[a1,a2,a3]:
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
\left.\mathrm a=[\mathrm e_1,\mathrm e_2,\mathrm e_3]\left[\begin{array}{c}\mathrm a_1\\\mathrm a_2\\\mathrm a_3\end{array}\right.\right]=\mathrm a_1\mathrm e_1+\mathrm a_2\mathrm e_2+\mathrm a_3\mathrm e_3
a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
2.内积/数量积/点积 & 外积/向量积/叉积
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
cos
⟨
a
,
b
⟩
\mathrm{a\cdot b=a^Tb=\sum_{i=1}^3a_ib_i=|a||b|\cos\langle a,b\rangle}
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
]
\left.\mathrm a\times\mathrm b=\left[\begin{array}{ccc}\mathrm i&\mathrm j&\mathrm k\\\mathrm a_1&\mathrm a_2&\mathrm a_3\\\mathrm b_1&\mathrm b_2&\mathrm b_3\end{array}\right.\right]=\left[\begin{array}{ccc}\mathrm a_2\mathrm b_3-\mathrm a_3\mathrm b_2\\\mathrm a_3\mathrm b_1-\mathrm a_1\mathrm b_3\\\mathrm a_1\mathrm b_2-\mathrm a_2\mathrm b_1\end{array}\right]
a×b=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤
3.反对称矩阵
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
\left.\mathrm{a}^{\wedge}=\left[\begin{array}{ccc}0&-\mathrm{a}_3&\mathrm{a}_2\\\mathrm{a}_3&0&-\mathrm{a}_1\\-\mathrm{a}_2&\mathrm{a}_1&0\end{array}\right.\right]
a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
附:其余线性代数基础请点击链接【速通线性代数】
4.四元数
- 四元数定义:类似于复数。1个实部+3个虚部
q = q 0 + q 1 i + q 2 j + q 3 k \mathrm q=\mathrm q_0+\mathrm q_1\mathrm i+\mathrm q_2\mathrm j+\mathrm q_3\mathrm k q=q0+q1i+q2j+q3k - 向量表示形式:
q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 \mathrm q=[\mathrm s,\mathrm v],\quad\mathrm s=\mathrm q_0\in\mathbb R,\quad\mathrm v=[\mathrm q_1,\mathrm q_2,\mathrm q_3]^\mathrm{T}\in\mathbb R^3 q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3 - 运算法则:
{ i 2 = j 2 = k 2 = − 1 i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j \left.\left\{\begin{aligned}\mathrm{i^2=j^2=k^2=-1}\\\mathrm{ij=k,ji=-k}\\\mathrm{jk=i,kj=-i}\\\mathrm{ki=j,ik=-j}\end{aligned}\right.\right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
附:其余四元数运算请点击链接【四元数的运算】 - 四元数表示旋转
类似于复变(复变量乘以 e i θ e^{i\theta} eiθ,将原向量逆时针旋转角度θ),可以用单位四元数( e i θ e^{i\theta} eiθ模长为1)表示旋转。
旋转度数 | 效果 |
---|---|
乘以 i / j / k | 旋转180° |
乘以i方 i^2=-1 | 旋转360°得到一个相反向量 |
旋转两周 | 与原先的样子相等 |
三、坐标的欧式变换
1.何为欧式变换
欧式变换:前后的两个坐标系下,同一个向量的模长和方向不发生改变。欧式变换=旋转+平移。
2.旋转矩阵
同一个向量a在空间中是不变的,但在不同的坐标系基底下对应的坐标值。
[
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.[\mathrm e_1,\mathrm e_2,\mathrm e_3]\left[\begin{array}{c}\mathrm a_1\\\mathrm a_2\\\mathrm a_3\end{array}\right.\right]=[\mathrm e_1',\mathrm e_2',\mathrm e_3']\left[\begin{array}{c}\mathrm a_1'\\\mathrm a_2'\\\mathrm a_3'\end{array}\right]
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤
等式左右两边同时左乘
[
e
1
T
,
e
2
T
,
e
3
T
]
T
[\mathrm{e}_1^\mathrm{T},\mathrm{e}_2^\mathrm{T},\mathrm{e}_3^\mathrm{T}]^\mathrm{T}
[e1T,e2T,e3T]T,可得:
[
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.\left[\begin{array}{c}\mathrm{a}_{1}\\\mathrm{a}_{2}\\\mathrm{a}_{3}\end{array}\right.\right]=\left[\begin{array}{ccc}\mathrm{e}_{1}^{\mathrm{T}}\mathrm{e}_{1}^{\prime}&\mathrm{e}_{1}^{\mathrm{T}}\mathrm{e}_{2}^{\prime}&\mathrm{e}_{1}^{\mathrm{T}}\mathrm{e}_{3}^{\prime}\\\mathrm{e}_{2}^{\mathrm{T}}\mathrm{e}_{1}^{\prime}&\mathrm{e}_{2}^{\mathrm{T}}\mathrm{e}_{2}^{\prime}&\mathrm{e}_{2}^{\mathrm{T}}\mathrm{e}_{3}^{\prime}\\\mathrm{e}_{3}^{\mathrm{T}}\mathrm{e}_{1}^{\prime}&\mathrm{e}_{3}^{\mathrm{T}}\mathrm{e}_{2}^{\prime}&\mathrm{e}_{3}^{\mathrm{T}}\mathrm{e}_{3}^{\prime}\end{array}\right]\left[\begin{array}{c}\mathrm{a}_{1}^{\prime}\\\mathrm{a}_{2}^{\prime}\\\mathrm{a}_{3}^{\prime}\end{array}\right]\triangleq\mathrm{Ra}^{\prime}
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
- 其中R为旋转矩阵.(旋转矩阵为正交矩阵,行向量/列向量是两两正交的单位向量,det|R|=1)
- 所有旋转矩阵构成特殊正交群SO(n): S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ( R ) = 1 } \mathrm{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}
3.欧式变换的向量表示
a ′ = R a + t \mathrm a'=\mathrm R\mathrm a+\mathrm t a′=Ra+t
4.变换矩阵与齐次坐标
由
a
′
=
R
a
+
t
\mathrm a'=\mathrm R\mathrm a+\mathrm t
a′=Ra+t进行多次变换,变换后的形式将会显得啰嗦,特此引入齐次坐标和变换矩阵。在三维向量的末尾添加1,构成的四维向量称为齐次坐标,将旋转和平移写入一个矩阵中,得到变换矩阵T。
[
a
′
1
]
=
[
R
t
0
1
]
[
a
1
]
≜
T
[
a
1
]
\left.\left[\begin{array}{c}\mathrm{a'}\\1\end{array}\right.\right]=\left[\begin{array}{cc}\mathrm{R}&\mathrm{t}\\0&1\end{array}\right]\left[\begin{array}{c}\mathrm{a}\\1\end{array}\right]\triangleq\mathrm{T}\left[\begin{array}{c}\mathrm{a}\\1\end{array}\right]
[a′1]=[R0t1][a1]≜T[a1]
- 特殊欧式群SE(3): S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } \left.\left.\mathrm{SE(3)}=\left\{\mathrm{T}=\left[\begin{array}{cc}\mathrm{R}&\mathrm{t}\\0&1\end{array}\right.\right.\right]\in\mathbb{R}^{4\times4}|\mathrm{R}\in\mathrm{SO(3)},\mathrm{t}\in\mathbb{R}^3\right\} SE(3)={T=[R0t1]∈R4×4∣R∈SO(3),t∈R3}
四、旋转的不同表现形式
表示形式 | 数学表达式 |
---|---|
旋转矩阵 | [ 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 ′ ] \left.\left[\begin{array}{ccc}\mathrm{e}_1^\mathrm{T}\mathrm{e}_1^{\prime}&\mathrm{e}_1^\mathrm{T}\mathrm{e}_2^{\prime}&\mathrm{e}_1^\mathrm{T}\mathrm{e}_3^{\prime}\\\mathrm{e}_2^\mathrm{T}\mathrm{e}_1^{\prime}&\mathrm{e}_2^\mathrm{T}\mathrm{e}_2^{\prime}&\mathrm{e}_2^\mathrm{T}\mathrm{e}_3^{\prime}\\\mathrm{e}_3^\mathrm{T}\mathrm{e}_1^{\prime}&\mathrm{e}_3^\mathrm{T}\mathrm{e}_2^{\prime}&\mathrm{e}_3^\mathrm{T}\mathrm{e}_3^{\prime}\end{array}\right.\right] ⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤ |
旋转向量/轴角Axis-Angle(存在奇异性) | θn(θ-旋转角度 n-旋转轴向量) |
欧拉角(直观但存在万向锁问题) | [r,p,y] ^T (rpy角的旋转顺序为ZYX) |
四元数(无奇异性) | q = [ s , v ] , s = q 0 ∈ R , v = [ q 1 , q 2 , q 3 ] T ∈ R 3 \mathrm q=[\mathrm s,\mathrm v],\quad\mathrm s=\mathrm q_0\in\mathbb R\quad\mathrm ,v=[\mathrm q_1,\mathrm q_2,\mathrm q_3]^{\mathrm T}\in\mathbb R^3 q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3 |
五、旋转不同表现形式之间的转换
转换 | 公式 |
---|---|
旋转向量到旋转矩阵 | R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ \mathrm{R}=\cos\theta\mathrm{I}+(1-\cos\theta)\mathrm{n}\mathrm{n}^\mathrm{T}+\sin\theta\mathrm{~n}^\mathrm{\wedge} R=cosθI+(1−cosθ)nnT+sinθ n∧ |
旋转向量到单位四元数 | q = [ cos ( θ 2 ) , n sin ( θ 2 ) ] T = [ cos ( θ 2 ) , n x sin ( θ 2 ) , n y sin ( θ 2 ) , n z sin ( θ 2 ) ] T \mathrm q=\left[\cos(\frac{\theta}{2}),\mathrm n\sin(\frac{\theta}{2})\right]^\mathrm{T}=\left[\cos(\frac{\theta}{2}),\mathrm n_\mathrm{x}\sin(\frac{\theta}{2}),\mathrm n_\mathrm{y}\sin(\frac{\theta}{2}),\mathrm n_z\sin(\frac{\theta}{2})\right]^\mathrm{T} q=[cos(2θ),nsin(2θ)]T=[cos(2θ),nxsin(2θ),nysin(2θ),nzsin(2θ)]T |
旋转矩阵到旋转向量 | 旋转角 θ = arccos ( t r ( R ) − 1 2 ) 旋转轴n是矩阵R特征值 1 对应的特征向量 \begin{aligned}&\text{旋转角}\theta=\arccos\left(\frac{\mathrm{tr}(\mathrm{R})-1}2\right)\\&\text{旋转轴n是矩阵R特征值}1\text{对应的特征向量}\end{aligned} 旋转角θ=arccos(2tr(R)−1)旋转轴n是矩阵R特征值1对应的特征向量 |
单位四元数到旋转向量* | { θ = 2 arccos q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin θ 2 \left.\left\{\begin{array}{l}\theta=2\arccos\mathrm{q_0}\\[2ex][\mathrm{n_x},\mathrm{n_y},\mathrm{n_z}]=[\mathrm{q_1},\mathrm{q_2},\mathrm{q_3}]^\mathrm{T}/\sin\frac{\theta}{2}\end{array}\right.\right. {θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sin2θ |
单位四元数到旋转矩阵 | R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R=vv^{T}+s^{2}I+2sv^\mathrm{\wedge}+\left(v^\mathrm{\wedge}\right)^{2} R=vvT+s2I+2sv∧+(v∧)2 |
六、常见变换的性质比较
笔者对仿射变换和射影变换的了解较少,仅列出有一印象。
变换名称 | 不变性质 |
---|---|
欧式变换 | 长度、夹角、体积 |
相似变换 | 体积比 |
仿射变换 | 平行性、体积比 |
射影变换 | 接触平面的相交和相切 |
七、坐标变换的例子
设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: q1 = [0.35, 0.2, 0.3, 0.1], t1 = [0.3, 0.1, 0.1],小萝卜二号的位姿为 q2 = [−0.5, 0.4, −0.1, 0.2], t2 = [−0.1, 0.5, 0.3] (q 的第一项为实部)。这里的 q 和 t 表达的是世界到相机的变换关系。现在,小萝卜一号看到某个点在自身的坐标系下的坐标为 p1 = [0.5, 0, 0.2] ,求该向量在小萝卜二号坐标系下的坐标。
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core> //Eigen的核心部分
#include <Eigen/Geometry> //Eigen的几何模块
using namespace std;
using namespace Eigen;
int main(int argc, char** argv) {
Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
q1.normalize(); //四元数单位化,单位四元数表示旋转
q2.normalize();
Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3); //平移向量
Vector3d p1(0.5, 0, 0.2); //点在小萝卜1号坐标系下的坐标
Isometry3d T1w(q1), T2w(q2); //旋转矩阵(自动把四元数转化为旋转矩阵)和平移向量构成变换矩阵
T1w.pretranslate(t1);
T2w.pretranslate(t2);
Vector3d p2 = T2w * T1w.inverse() * p1; //T1w.inverse() * p1把点在小萝卜1号下的坐标转换到世界坐标系;T2w *把点在世界坐标系下的坐标转换到小萝卜2号坐标系中
cout << endl << p2.transpose() << endl; //转换后的坐标转置输出
return 0;
}