标签：prime right 笔记 旋转 SLAM 视觉 array 向量 left

目录

文章目录

• 目录
• 第1讲 引言
• 第2讲 初识SLAM
• 第3讲 三维空间刚体运动
• • 旋转矩阵
  • • 点，向量和坐标系
    • 坐标系间的欧式变换
    • 变换矩阵与齐次坐标
    • 齐次坐标(Homogeneous Coordinate)的优势
  • 旋转向量和欧拉角
  • • 旋转向量
    • 欧拉角
  • 四元数
  • • 四元数定义
    • 用单位四元数表示旋转

第1讲 引言

SLAM是Simultaneous Localization and Mapping的缩写，中文译作同时定位与地图构建。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。

习题答案汇总：链接

第2讲 初识SLAM

自主运动两大基本问题：我在什么地方？（定位）周围什么样子？（建图）

How to SLAM？——Sensors

两类传感器：

• 安装于环境中的：二维码，GPS，导轨、磁条
• 携带于机器人本体上：IMU，激光，相机

相机分类：

• 单目 Monocular，没有深度，必须通过移动相机产生深度moving view stereo
• 双目 Stereo，通过视差计算深度
• 深度 RGBD，通过物理方法测量深度
• 其他 鱼眼 全景 Event Camera，etc

整体视觉SLAM流程图如下：

• 前端：Visual Odometry
• 后端：optimization
• 回环检测 Loop Closing
• 建图 Mapping

视觉里程计VO：又称为前端

• 相邻图像估计相机运动
• 通过两张图像计算运动和结构
• 不可避免地有漂移

后端优化：
-从带有噪声的数据中优化轨迹和地图

• 最大后验概率MAP
• 前期EKF为代表，现在图优化为代表

回环检测：

• 检测机器人是否回到早先位置
• 识别到达过的场景
• 计算图像间相似性

建图：

• 用于导航、规划、通讯、可视化、交互等
• 度量地图VS拓扑地图
• 稀疏地图VS稠密地图

SLAM问题的数学表述：

小萝卜携带着传感器在环境中运动”，由如下两件事情描述:
1.什么是运动 ?我们要考虑从 k − 1 k-1 k−1时刻到 k k k时刻，小萝卜的位置 x x x是如何变化的。
运动方程:
x k = f ( x k − 1 , u k , w k ) x_k = f(x_{k-1},u_k,w_k) xk​=f(xk−1​,uk​,wk​)

• x k x_k xk​， x k − 1 x_{k-1} xk−1​ 表示小萝卜在 k k k和 k − 1 k−1 k−1 时刻的位置
• u k u_k uk​表示运动传感器的读数(有时也叫输入)
• w k w_k wk​表示噪声

2.什么是观测 ?假设小萝卜在k时刻于 x k x_k xk​处探测到了某一个路标 y j y_j yj​，我们要考虑这件事情是如何用数学语言来描述的。
z k , j = h ( y j , x k , v k , j ) z_{k,j} = h(y_j,x_k,v_{k,j}) zk,j​=h(yj​,xk​,vk,j​)

• z k , j z_{k,j} zk,j​​表示小萝卜在 x k x_k xk​位置上看到路标点 y j y_j yj​产生的观测数据
• y j y_j yj​​表示第 j j j个路标点
• v k , j v_{k, j} vk,j​表示噪声

这两个方程描述了最基本的SLAM问题：当知道运动测量的读数 u u u，以及传感器的读数 z z z时,如何求解定位问题(估计 x x x)和建图问题(估计 y y y)？这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

第3讲 三维空间刚体运动

旋转矩阵

点，向量和坐标系

1. 向量 α \alpha α在线性空间的基 [ e 1 , e 2 , e 3 ] [e_{1},e_{2},e_{3}] [e1​,e2​,e3​]下的坐标为 [ α 1 , α 2 , α 3 ] ⊤ [\alpha_{1},\alpha_{2},\alpha_{3}]^\top [α1​,α2​,α3​]⊤

2. 向量的内积: 描述向量间的投影关系
   a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ cos ⁡ ⟨ a , b ⟩ a \cdot b=a^{T} b=\sum_{i=1}^{3} a_{i} b_{i}=|a||b| \cos \langle a, b\rangle a⋅b=aTb=i=1∑3​ai​bi​=∣a∣∣b∣cos⟨a,b⟩

3. 向量的外积: 描述向量的旋转
   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}{ccc} i & j & k \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right]=\left[\begin{array}{c} a_{2} b_{3}-a_{3} b_{2} \\ a_{3} b_{1}-a_{1} b_{3} \\ a_{1} b_{2}-a_{2} b_{1} \end{array}\right]=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] b \triangleq a^{\wedge} b a×b=⎣⎡​ia1​b1​​ja2​b2​​ka3​b3​​⎦⎤​=⎣⎡​a2​b3​−a3​b2​a3​b1​−a1​b3​a1​b2​−a2​b1​​⎦⎤​=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​b≜a∧b

其中 a ∧ a^{\wedge} a∧ 表示 a a a 的反对称矩阵
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a^{\wedge}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right] a∧=⎣⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦⎤​

坐标系间的欧式变换

1. 欧式变换:在欧式变换前后的两个坐标系下,同一个向量的模长和方向不发生改变,是为欧式变换.一个欧式变换由一个旋转和一个平移组成。
2. 旋转矩阵 R R R

• 旋转矩阵 R R R 的推导:
  设单位正交基 [ e 1 , e 2 , e 3 ] \left[e_{1}, e_{2}, e_{3}\right] [e1​,e2​,e3​] 经过一次旋转变成了 [ e 1 ′ , e 2 ′ , e 3 ′ ] \left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right] [e1′​,e2′​,e3′​], 对于同一个向量 a a a, 在两个坐标系下的坐标分别为 [ a 1 , a 2 , a 3 ] T \left[a_{1}, a_{2}, a_{3}\right]^{T} [a1​,a2​,a3​]T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T \left[a_{1}^{\prime}, a_{2}^{\prime}, a_{3}^{\prime}\right]^{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[e_{1}, e_{2}, e_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[e_{1}^{\prime}, e_{2}^{\prime}, e_{3}^{\prime}\right]\left[\begin{array}{l} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] [e1​,e2​,e3​]⎣⎡​a1​a2​a3​​⎦⎤​=[e1′​,e2′​,e3′​]⎣⎡​a1′​a2′​a3′​​⎦⎤​
  等式左右两边同时左乘 [ e 1 T , e 2 T , e 3 T ] T \left[e_{1}^{T}, e_{2}^{T}, e_{3}^{T}\right]^{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[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{ccc} e_{1}^{T} e_{1}^{\prime} & e_{1}^{T} e_{2}^{\prime} & e_{1}^{T} e_{3}^{\prime} \\ e_{2}^{T} e_{1}^{\prime} & e_{2}^{T} e_{2}^{\prime} & e_{2}^{T} e_{3}^{\prime} \\ e_{3}^{T} e_{1}^{\prime} & e_{3}^{T} e_{2}^{\prime} & e_{3}^{T} e_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \triangleq R a^{\prime} ⎣⎡​a1​a2​a3​​⎦⎤​=⎣⎡​e1T​e1′​e2T​e1′​e3T​e1′​​e1T​e2′​e2T​e2′​e3T​e2′​​e1T​e3′​e2T​e3′​e3T​e3′​​⎦⎤​⎣⎡​a1′​a2′​a3′​​⎦⎤​≜Ra′
  矩阵 R R R 描述了旋转称为旋转矩阵。

• 旋转矩阵 R R R的性质

  • 旋转矩阵是行列式为1的正交矩阵，任何行列式为1的正交矩阵也是一个旋转矩阵。所有旋转矩阵构成特殊正交群 S O SO SO
    S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } S O(n)=\left\{R \in \mathbb{R}^{n \times n} \mid R R^{T}=I, \operatorname{det}(R)=1\right\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

  • 旋转矩阵是正交矩阵(其转置等于其逆), 旋转矩阵的逆 R − 1 R^{-1} R−1 (即转置 R T R^{T} RT )描述了一个相反的旋转。

3. 欧式变换的向量表示
   世界坐标系中的向量 a a a, 经过一次旋转(用旋转矩阵 R R R 描述)和一次平移(用平移向量 t t t 描述)后,得到了 a ′ a^{\prime} a′ :
   a ′ = R a + t a^{\prime}=R a+t a′=Ra+t

变换矩阵与齐次坐标

1. 变换矩阵 T T T :
   在三维向量的末尾添加 1 , 构成的四维向量称为齐次坐标将旋转和平移写入变换矩阵 T T T 中,得到:
   [ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{c} a^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right]\left[\begin{array}{l} a \\ 1 \end{array}\right] \triangleq T\left[\begin{array}{l} a \\ 1 \end{array}\right] [a′1​]=[R0​t1​][a1​]≜T[a1​]
   齐次坐标的意义在于将欧式变换表示为线性关系。

2. 变换矩阵 T T T 的性质:

• 变换矩阵 T T T 构成特殊欧式群 S E S E SE
  S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } S E(3)=\left\{T=\left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in S O(3), t \in \mathbb{R}^{3}\right\} SE(3)={T=[R0​t1​]∈R4×4∣R∈SO(3),t∈R3}
• 变换矩阵的逆表示一个反向的欧式变换
  T − 1 = [ R T − R T t 0 1 ] T^{-1}=\left[\begin{array}{cc} R^{T} & -R^{T} t \\ 0 & 1 \end{array}\right] T−1=[RT0​−RTt1​]

齐次坐标(Homogeneous Coordinate)的优势

1. 方便判断是否在直线或平面上
   若点 p = ( x , y ) p=(x, y) p=(x,y) 在直线 l = ( a , b , c ) l=(a, b, c) l=(a,b,c) 上, 则有:
   a x + b y + c = [ a , b , c ] T ⋅ [ x , y , 1 ] = l T ⋅ p ′ = 0 a x+b y+c=[a, b, c]^{T} \cdot[x, y, 1]=l^{T} \cdot p^{\prime}=0 ax+by+c=[a,b,c]T⋅[x,y,1]=lT⋅p′=0
   若点 p = ( x , y , z ) p=(x, y, z) p=(x,y,z) 在平面 A = ( a , b , c , d ) A=(a, b, c, d) A=(a,b,c,d) 上, 则有:
   a x + b y + c z + d = [ a , b , c , d ] T ⋅ [ x , y , z , 1 ] = A T ⋅ p ′ = 0 a x+b y+c z+d=[a, b, c, d]^{T} \cdot[x, y, z, 1]=A^{T} \cdot p^{\prime}=0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0

2. 方便表示线线交点和点点共线
   在齐次坐标下,
   性质1：可以用两个点 p p p， q q q的齐次坐标叉乘结果表示它们的共线 l l l
   性质2：可以用两条直线 l l l， m m m的齐次坐标叉乘结果表示它们的交点 x x x
   这里利用了叉乘的性质: 叉乘结果与两个运算向量都垂直。

• 性质1的证明:
  l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 \begin{aligned} &l^{T} \cdot p=(p \times q) \cdot p=0 \\ &l^{T} \cdot q=(p \times q) \cdot q=0 \end{aligned} ​lT⋅p=(p×q)⋅p=0lT⋅q=(p×q)⋅q=0​
• 性质2的证明:
  l T ⋅ p = l T ⋅ ( l × m ) = 0 m T ⋅ p = m T ⋅ ( l × m ) = 0 \begin{aligned} l^{T} \cdot p &=l^{T} \cdot(l \times m)=0 \\ m^{T} \cdot p &=m^{T} \cdot(l \times m)=0 \end{aligned} lT⋅pmT⋅p​=lT⋅(l×m)=0=mT⋅(l×m)=0​

3. 能够区分向量和点

• 点 ( x , y , z ) (x, y, z) (x,y,z) 的齐次坐标为 ( x , y , z , 1 ) (x, y, z, 1) (x,y,z,1)
• 向量 ( x , y , z ) (x, y, z) (x,y,z) 的齐次坐标为 ( x , y , z , 0 ) (x, y, z, 0) (x,y,z,0)

4. 能够表达无穷远点
   对于平行直线 l = ( a , b , c ) l=(a, b, c) l=(a,b,c) 和 m = ( a , b , d ) m=(a, b, d) m=(a,b,d), 求取其交点的齐次坐标 x = l × m = ( k b , − k a , 0 ) x=l \times m=(k b,-k a, 0) x=l×m=(kb,−ka,0), 将其转为非齐次坐标,得到 x = x= x= ( k b / 0 , − k a / 0 ) = ( inf ⁡ , − inf ⁡ ) (k b / 0,-k a / 0)=(\inf ,-\inf ) (kb/0,−ka/0)=(inf,−inf), 这表示无穷远点.

5. 能够简洁的表示变换
   使用齐次坐标, 可以将加法运算转化为乘法运算。
   

旋转向量和欧拉角

旋转向量

1. 旋转矩阵的缺点:
   1. 旋转矩阵有9个量,但一次旋转只有3个自由度,这种表达方式是冗余的。
   2. 旋转矩阵自带约束(必须是行列式为1的正交矩阵),这些约束会给估计和优化带来困难。
2. 旋转向量：任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向表示旋转轴而长度表示旋转角.这种向量称为旋转向量(或轴角，Axis-Angle)。

假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn。

3. 旋转向量和旋转矩阵之间的转换:

设旋转向量 R R R表示一个绕单位向量 n n n，角度为 θ \theta θ的旋转。

• 旋转向量到旋转矩阵:
  R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ n ∧ R=\cos \theta I+(1-\cos \theta) n n^{T}+\sin \theta n^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
• 旋转矩阵到旋转向量:
  • 旋转角 θ = arccos ⁡ ( tr ⁡ ( R ) − 1 2 ) \theta=\arccos \left(\frac{\operatorname{tr}(R)-1}{2}\right) θ=arccos(2tr(R)−1​)
  • 旋转轴 n n n 是矩阵 R R R 特征值 1 对应的特征向量

欧拉角

1. 欧拉角将一次旋转分解成3个分离的转角.常用的一种ZYX转角将任意旋转分解成以下3个轴上的转角：
   • 绕物体的Z ZZ轴旋转,得到偏航角yaw
   • 绕旋转之后的Y YY轴旋转,得到俯仰角pitch
   • 绕旋转之后的X XX轴旋转,得到滚转角roll
2. 欧拉角的一个重大缺点是万向锁问题(奇异性问题):在俯仰角为90° 时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由3次旋转变成了2次旋转)。

四元数

为什么需要四元数：对于三维旋转，找不到不带奇异性的三维向量描述方式。因此引入四元数。四元数是一种扩展的复数，既是紧凑的,也没有奇异性。

四元数定义

1. 四元数的定义
   一个四元数 q q q 拥有一个实部和三个虚部
   q = q 0 + q 1 i + q 2 j + q 3 k q=q_{0}+q_{1} i+q_{2} j+q_{3} k q=q0​+q1​i+q2​j+q3​k
   其中 i , j , k i, j, k i,j,k, 为四元数的 3 个虚部,它们满足以下关系式(自己和自己的运算像复数,自己和别人的运算像叉乘):
   { 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\{\begin{array}{l} 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 \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧​i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j​
   也可以用一个标量和一个向量来表达四元数:
   q = [ s , v ] , s = q 0 ∈ R v = [ q 1 , q 2 , q 3 ] T ∈ R 3 q=[s, v], \quad s=q_{0} \in \mathbb{R} \quad v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in \mathbb{R}^{3} q=[s,v],s=q0​∈Rv=[q1​,q2​,q3​]T∈R3
   s s s 为四元数的实部, v v v 为四元数的虚部。有实四元数和虚四元数的概念。

2. 四元数与旋转角度的关系:

   1. 在二维情况下, 任意一个旋转都可以用单位复数来描述, 乘 i i i 就是绕 i i i 轴旋转 9 0 ∘ 90^{\circ} 90∘ 。
   2. 在三维情况下, 任意一个旋转都可以用单位四元数来描述,乘 i i i 就是绕 i i i 轴旋转 18 0 ∘ 180^{\circ} 180∘ 。

3. 单位四元数和旋转向量之间的转换:
   设单位四元数 q q q 表示一个绕单位向量 n = [ n x , n y , n z ] T n=\left[n_{x}, n_{y}, n_{z}\right]^{T} n=[nx​,ny​,nz​]T, 角度为 θ \theta θ 的旋转。

   1. 从旋转向量到单位四元数:
      q = [ cos ⁡ ( θ 2 ) , n sin ⁡ ( θ 2 ) ] T = [ cos ⁡ ( θ 2 ) , n x sin ⁡ ( θ 2 ) , n y sin ⁡ ( θ 2 ) , n z sin ⁡ ( θ 2 ) ] T q=\left[\cos \left(\frac{\theta}{2}\right), n \sin \left(\frac{\theta}{2}\right)\right]^{T}=\left[\cos \left(\frac{\theta}{2}\right), n_{x} \sin \left(\frac{\theta}{2}\right), n_{y} \sin \left(\frac{\theta}{2}\right), n_{z} \sin \left(\frac{\theta}{2}\right)\right]^{T} q=[cos(2θ​),nsin(2θ​)]T=[cos(2θ​),nx​sin(2θ​),ny​sin(2θ​),nz​sin(2θ​)]T
   2. 从单位四元数到旋转向量:
      { θ = 2 arccos ⁡ q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin ⁡ θ 2 \left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]=\left[q_{1}, q_{2}, q_{3}\right]^{T} / \sin \frac{\theta}{2}} \end{array}\right. {θ=2arccosq0​[nx​,ny​,nz​]=[q1​,q2​,q3​]T/sin2θ​​

用单位四元数表示旋转

给定一个空间三维点 p = [ x , y , z ] ∈ R 3 p=[x, y, z] \in \mathbb{R}^{3} p=[x,y,z]∈R3,以及一个由轴角 n , θ n, \theta n,θ 指定的旋转,三维点 p p p 经过旋箦后变为 p ′ p^{\prime} p′ 。如何使用单位四元数 q q q 表达旋转?

1. 把三维空间点用一个虚四元数 p p p 表示:
   p = [ 0 , x , y , z ] = [ 0 , v ] p=[0, x, y, z]=[0, v] p=[0,x,y,z]=[0,v]
2. 把旋转用单位四元数 q q q 表示:
   q = [ cos ⁡ θ 2 , n sin ⁡ θ 2 ] q=\left[\cos \frac{\theta}{2}, n \sin \frac{\theta}{2}\right] q=[cos2θ​,nsin2θ​]
3. 旋转后的点 p ′ p^{\prime} p′ 可表示为:
   p ′ = q p q − 1 p^{\prime}=q p q^{-1} p′=qpq−1

注意：只有单位四元数才能表示旋转，因此在程序中创建四元数后，记得调用normalize()将其单位化。

标签：prime,right,笔记,旋转,SLAM,视觉,array,向量,left
来源： https://blog.csdn.net/YuhsiHu/article/details/122444320

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。