《视觉SLAM十四讲》笔记摘抄

ch02 初识SLAM
- 经典视觉SLAM框架
- SLAM问题的数学表述
ch03 三维空间刚体运动
ch04 李群与李代数
ch05 相机与图像
ch06 非线性优化
ch07 视觉里程计01

ch02 初识SLAM

经典视觉SLAM框架

在这里插入图片描述

视觉SLAM流程包括以下步骤:

传感器信息读取: 在视觉SLAM中主要为相机图像信息的读取和预处理.如果是在机器人中,还可能有码盘、惯性传感器等信息的读取和同步.
视觉里程计(Visual Odometry,VO): 视觉里程计的任务是估算相邻图像间相机的运动,以及局部地图的样子.VO又称为前端(Front End).

视觉里程计不可避免地会出现累积漂移(Accumulating Drift)问题.
后端优化 (Optimization): 后端接受不同时刻视觉里程计测量的相机位姿,以及回环检测的信息,对它们进行优化,得到全局一致的轨迹和地图.由于接在VO之后,又称为后端(Back End).

在视觉 SLAM中,前端和计算机视觉研究领域更为相关,比如图像的特征提取与匹配等,后端则主要是滤波与非线性优化算法.
回环检测 (Loop Closing): 回环检测判断机器人是否到达过先前的位置.如果检测到回环,它会把信息提供给后端进行处理.
建图 (Mapping): 它根据估计的轨迹,建立与任务要求对应的地图.

地图的形式包括度量地图(精确表示地图物体的位置关系)与拓扑地图(更强调地图元素之间的关
系)两种.

SLAM问题的数学表述

“小萝卜携带着传感器在环境中运动”,由如下两件事情描述:

什么是运动 ?我们要考虑从 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 − 1 x_k, x_{k-1} xk,xk−1表示小萝卜在 k k k和 k − 1 k-1 k−1时刻的位置
- u k u_k uk表示运动传感器的读数(有时也叫输入)
- w k w_k wk表示噪声
什么是观测 ?假设小萝卜在 k k 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问题建模成了一个状态估计问题:如何通过带有噪声的测量数据,估计内部的、隐藏着的状态变量?

ch03 三维空间刚体运动

旋转矩阵

点和向量,坐标系

向量 a a a在线性空间的基 [ e 1 , e 2 , e 3 ] [e_1, e_2, e_3] [e1,e2,e3]下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1, a_2, a_3]^T [a1,a2,a3]T.

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{array}{c} a_1 \\ a_2 \\ a_3 \end{array}\right] = a_1e_1 + a_2e_2 + a_3e_3 a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3
向量的内积与外积
- 向量的内积: 描述向量间的投影关系
  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_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 ] = [ 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_2b_3 - a_3b_2 \\ a_3b_1 - a_1b_3 \\ a_1b_2 - a_2b_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=⎣⎡ia1b1ja2b2ka3b3⎦⎤=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤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−a30a1a2−a10⎦⎤

坐标系间的欧氏变换

欧式变换:

在欧式变换前后的两个坐标系下,同一个向量的模长和方向不发生改变,是为欧式变换.

一个欧式变换由一个旋转和一个平移组成.
旋转矩阵 R R R:
- 旋转矩阵 R R R的推导:
  
  设单位正交基 [ 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 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{array}{c} a_1 \\ a_2 \\ a_3 \end{array}\right] = [e_1’, e_2’, e_3’] \left[\begin{array}{c} a_1’ \\ a_2’ \\ 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 [e_1^T, e_2^T, e_3^T]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}{c} a_1 \\ a_2 \\ a_3 \end{array}\right] = \left[\begin{array}{ccc} 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{array}\right] \left[\begin{array}{c} a_1’ \\ a_2’ \\ a_3’ \end{array}\right] \triangleq R a’ ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤≜Ra′
  
  矩阵 R R R描述了旋转,称为旋转矩阵.
- 旋转矩阵 R R R的性质
  1. 旋转矩阵是行列式为1的正交矩阵,任何行列式为1的正交矩阵也是一个旋转矩阵.所有旋转矩阵构成特殊正交群 S O SO SO:
  S O ( n ) = { R ∈ R n × n ∣ R R T = I , det ⁡ ( R ) = 1 } SO(n) = \{ R \in \mathbb{R}^{n \times n} | RR^T = I, \det®=1 \} SO(n)={ R∈Rn×n∣RRT=I,det®=1}
  1. 旋转矩阵是正交矩阵(其转置等于其逆),旋转矩阵的逆 R − 1 R^{-1} R−1(即转置 R T R^T RT)描述了一个相反的旋转.
欧式变换的向量表示:

世界坐标系中的向量 a a a,经过一次旋转(用旋转矩阵 R R R描述)和一次平移(用平移向量 t t t描述)后,得到了 a ′ a’ a′:
a ′ = R a + t a’ = Ra + t a′=Ra+t

变换矩阵与齐次坐标

变换矩阵 T T T:

在三维向量的末尾添加1,构成的四维向量称为齐次坐标.将旋转和平移写入变换矩阵 T T T中,得到:

[ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] ≜ T [ a 1 ] \left[\begin{array}{c} a’ \\ 1 \end{array}\right] = \left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right] \left[\begin{array}{c} a \\ 1 \end{array}\right] \triangleq T \left[\begin{array}{c} a \\ 1 \end{array}\right] [a′1]=[R0t1][a1]≜T[a1]
齐次坐标的意义在于将欧式变换表示为线性关系.
变换矩阵 T T T的性质:
1. 变换矩阵 T T T构成特殊欧式群 S E SE SE
  S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left\{ T = \left[\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right] \in \mathbb{R}^{4\times4} | R \in SO(3), t \in \mathbb{R}^3 \right\} SE(3)={ T=[R0t1]∈R4×4∣R∈SO(3),t∈R3}
2. 变换矩阵的逆表示一个反向的欧式变换
  T − 1 = [ R T − R T t 0 1 ] T^{-1} = \left[\begin{array}{cc} R^T & -R^Tt \\ 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 ax+by+c = [a,b,c]^T \cdot [x,y,1] = l^T \cdot p’ = 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 ax+by+cz+d = [a,b,c,d]^T \cdot [x,y,z,1] = A^T \cdot p’ = 0 ax+by+cz+d=[a,b,c,d]T⋅[x,y,z,1]=AT⋅p′=0

优势2:方便表示线线交点和点点共线

在齐次坐标下,

可以用两个点 p p p, q q q的齐次坐标叉乘结果表示它们的共线 l l l.
可以用两条直线 l l l, m m m的齐次坐标叉乘结果表示它们的交点 x x x.

在这里插入图片描述

这里利用叉乘的性质: 叉乘结果与两个运算向量都垂直:

性质1的证明:
l T ⋅ p = ( p × q ) ⋅ p = 0 l T ⋅ q = ( p × q ) ⋅ q = 0 l^T \cdot p = (p \times q) \cdot p = 0 \\ l^T \cdot q = (p \times q) \cdot q = 0 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 l^T \cdot p = l^T \cdot (l \times m) = 0 \\ m^T \cdot p = m^T \cdot (l \times m) = 0 lT⋅p=lT⋅(l×m)=0mT⋅p=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=(kb, -ka, 0) x=l×m=(kb,−ka,0),将其转为非齐次坐标,得到 x = ( k b / 0 , − k a / 0 ) = ( inf ⁡ , − inf ⁡ ) x = (kb/0, -ka/0) = (\inf, -\inf) x=(kb/0,−ka/0)=(inf,−inf),这表示无穷远点.

优势5:能够简洁的表示变换

使用齐次坐标,可以将加法运算转化为乘法运算.

变换形式	数学变换	MATLAB函数
位移(Translation)	[ x ′ y ′ 1 ] = [ 1 0 t x 0 1 t y 0 0 1 ] ∗ [ x y 1 ] \left[\begin{array}{c} x' \\ y' \\ 1 \end{array}\right] =\left[\begin{array}{c} 1 & 0 & t_x \\ 0 & 1 & t_y \\ 0 & 0 & 1 \end{array}\right] * \left[\begin{array}{c} x \\ y \\ 1 \end{array}\right] ⎣⎡x′y′1⎦⎤=⎣⎡100010txty1⎦⎤∗⎣⎡xy1⎦⎤	`imtranslate()`
缩放(Scale)	[ x ′ y ′ 1 ] = [ s x 0 0 0 s y 0 0 0 1 ] ∗ [ x y 1 ] \left[\begin{array}{c} x' \\ y' \\ 1 \end{array}\right] =\left[\begin{array}{c} s_x & 0 & 0 \\ 0 & s_y & 0 \\ 0 & 0 & 1 \end{array}\right] * \left[\begin{array}{c} x \\ y \\ 1 \end{array}\right] ⎣⎡x′y′1⎦⎤=⎣⎡sx000sy0001⎦⎤∗⎣⎡xy1⎦⎤	`imresize()`
错切(Shear)	[ x ′ y ′ 1 ] = [ 1 h x 0 h y 1 0 0 0 1 ] ∗ [ x y 1 ] \left[\begin{array}{c} x' \\ y' \\ 1 \end{array}\right] =\left[\begin{array}{c} 1 & h_x & 0 \\ h_y & 1 & 0 \\ 0 & 0 & 1 \end{array}\right] * \left[\begin{array}{c} x \\ y \\ 1 \end{array}\right] ⎣⎡x′y′1⎦⎤=⎣⎡1hy0hx10001⎦⎤∗⎣⎡xy1⎦⎤
旋转(Rotate)	[ x ′ y ′ 1 ] = [ cos ⁡ θ sin ⁡ θ 0 − sin ⁡ θ cos ⁡ θ 0 0 0 1 ] ∗ [ x y 1 ] \left[\begin{array}{c} x' \\ y' \\ 1 \end{array}\right] =\left[\begin{array}{c} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{array}\right] * \left[\begin{array}{c} x \\ y \\ 1 \end{array}\right] ⎣⎡x′y′1⎦⎤=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤∗⎣⎡xy1⎦⎤	`imrotate()`

旋转向量和欧拉角

旋转向量

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

假设有一个旋转轴为 n n n,角度为 θ \theta θ的旋转,其对应的旋转向量为 θ n \theta n θn.
旋转向量和旋转矩阵之间的转换:

设旋转向量 R R R表示一个绕单位向量 n n n,角度为 θ θ θ的旋转.
- 旋转向量到旋转矩阵:
  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 ⁡ ( t r ( R ) − 1 2 ) \theta = \arccos \left( \frac{tr®-1}{2} \right) θ=arccos(2tr®−1)
  - 旋转轴 n n n是矩阵 R R R特征值1对应的特征向量

欧拉角

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

四元数

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

四元数的定义

四元数的定义

一个四元数 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+q1i+q2j+q3k

其中 i i i, j j j, k k 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{aligned} & i^2 = j^2 = k^2 = -1 \\ & ij = k, ji=-k \\ & jk = i, kj=-i \\ & ki = j, ik=-j \end{aligned} \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=[q_1, q_2, q_3]^T \in \mathbb{R}^3 q=[s,v],s=q0∈Rv=[q1,q2,q3]T∈R3

s s s为四元数的实部, v v v为四元数的虚部.有实四元数和虚四元数的概念.
四元数与旋转角度的关系:
- 在二维情况下,任意一个旋转都可以用单位复数来描述,乘 i i i就是绕 i i i轴旋转90°.
- 在三维情况下,任意一个旋转都可以用单位四元数来描述,乘 i i i就是绕 i i i轴旋转180°.
单位四元数和旋转向量之间的转换:

设单位四元数 q q q表示一个绕单位向量 n = [ n x , n y , n z ] T n =[n_x,n_y,n_z]^T n=[nx,ny,nz]T,角度为 θ θ θ的旋转.
- 从旋转向量到单位四元数:
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(\frac{\theta}{2}), n\sin(\frac{\theta}{2}) \right]^T= \left[ \cos(\frac{\theta}{2}), n_x\sin(\frac{\theta}{2}), n_y\sin(\frac{\theta}{2}), n_z\sin(\frac{\theta}{2}) \right]^T q=[cos(2θ),nsin(2θ)]T=[cos(2θ),nxsin(2θ),nysin(2θ),nzsin(2θ)]T
- 从单位四元数到旋转向量:
  { θ = 2 arccos ⁡ q 0 [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] T / sin ⁡ θ 2 \left\{ \begin{aligned} & \theta = 2 \arccos{q_0}\\ & [n_x,n_y,n_z] = [q_1, q_2, q_3]^T / \sin{\frac{\theta}{2}} \end{aligned} \right. ⎩⎨⎧θ=2arccosq0[nx,ny,nz]=[q1,q2,q3]T/sin2θ

用单位四元数表示旋转

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

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

这样得到的点 p ′ p’ p′仍为一个纯虚四元数,其虚部的3个分量表示旋转后3D点的坐标.

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

ch04 李群与李代数

李群与李代数基础

旋转矩阵构成特殊正交群 S O ( 3 ) SO(3) SO(3),变换矩阵构成了特殊欧氏群 S E ( 3 ) SE(3) SE(3).
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ⁡ ( R ) = 1 } S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } \begin{aligned} SO(3) &= \left\{ R \in \mathbb{R}^{3\times 3} | RR^T=I, \det®=1 \right\} \\ SE(3) &= \left\{ T = \left[\begin{array}{cc} R & t \\ 0^T & 1 \end{array}\right] \in \mathbb{R}^{4\times 4} | R \in SO(3), t \in \mathbb{R}^3 \right\} \end{aligned} SO(3)SE(3)={ R∈R3×3∣RRT=I,det®=1}={ T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}

群的定义

群(Group)是一种集合加上一种运算的代数结构.把集合记作 A A A,运算记作 ⋅ \cdot ⋅ ,那么群可以记作 G = ( A , ⋅ ) G =(A,\cdot) G=(A,⋅).群要求这个运算满足如下条件(封结幺逆):
1. 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A \forall{a_1, a_2} \in A, \quad a_1 \cdot a_2 \in A ∀a1,a2∈A,a1⋅a2∈A.
2. 结合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall{a_1, a_2, a_3} \in A, \quad (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3 ) ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)
3. 幺元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a \exists{a_0} \in A, \quad \mathrm{s.t.} \quad \forall a \in A, \quad a_0\cdot a = a\cdot a_0 = a ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a
4. 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 \forall a \in A, \quad \exists{a^{-1}} \in A, \quad \mathrm{s.t.} a\cdot a^{-1}=a_0 ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0
李群是指具有连续(光滑)性质的群. S O ( 3 ) SO(3) SO(3)和 S E ( 3 ) SE(3) SE(3)都是李群

李代数的定义

每个李群都有与之对应的李代数,李代数描述了李群的局部性质.

通用的李代数的定义如下:
李代数由一个集合 V V V,一个数域 F F F和一个二元运算 [ , ] [, ] [,]组成.如果它们满足以下几条性质,则称 ( V , F , [ , ] ) (V, F, [, ]) (V,F,[,])为一个李代数,记作 g \mathfrak{g} g.

封闭性: ∀ X , Y ∈ V , [ X , Y ] ∈ V \forall{X, Y} \in V, [X,Y] \in V ∀X,Y∈V,[X,Y]∈V.
双线性: $\forall X,Y,Z \in V, a,b \in F $有:
[ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] [aX+bY,Z]=a[X,Z]+b[Y,Z], \quad [Z, aX+bY]=a[Z,X]+b[Z,Y] [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]
自反性: ∀ X , ∈ V , [ X , X ] = 0 \forall{X,} \in V, [X,X]=0 ∀X,∈V,[X,X]=0.
雅可比等价 ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0 \forall X,Y,Z \in V, \quad [X, [Y,Z ]]+[Z, [X,Y ]]+[Y, [Z,X ]]=0 ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.

其中的二元运算 [ , ] [,] [,]被称为李括号.例如三维向量空间 R 3 \mathbb{R^3} R3上定义的叉积 × \times ×是一种李括号.

李代数 s o ( 3 ) \mathfrak{so}(3) so(3)

李群 S O ( 3 ) SO(3) SO(3)对应的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)是定义在 R 3 \mathbb{R^3} R3上的向量,记作 ϕ \phi ϕ.

s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 } \mathfrak{so}(3) = \left\{ \phi \in \mathbb{R^3}, \Phi=\phi^\wedge = \left[\begin{array}{ccc} 0 & -\phi_3 & \phi_2\\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{array}\right] \in \mathbb{R^{3 \times 3}} \right\} so(3)=⎩⎨⎧ϕ∈R3,Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3⎭⎬⎫
李代数 s o ( 3 ) \mathfrak{so}(3) so(3)的李括号为
[ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ [\phi_1, \phi_2] = (\Phi_1 \Phi_2 - \Phi_2 \Phi_1) ^\vee [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨
其中 ∨ ^\vee ∨是 ∧ ^\wedge ∧的逆运算,表示将反对称矩阵还原为向量
s o ( 3 ) \mathfrak{so}(3) so(3)和 S O ( 3 ) SO(3) SO(3)间的映射关系为

李群 R = exp ⁡ ( ϕ ∧ ) = exp ⁡ ( Φ ) 李代数 ϕ = ln ⁡ ( R ) ∨ \begin{aligned} 李群R &= \exp(\phi ^\wedge) = \exp (\Phi) \\ 李代数\phi &= \ln ® ^\vee \end{aligned} 李群R李代数ϕ=exp(ϕ∧)=exp(Φ)=ln®∨

李代数 s e ( 3 ) \mathfrak{se}(3) se(3)

类似地,李群 S E ( 3 ) SE(3) SE(3)的李代数 s e ( 3 ) \mathfrak{se}(3) se(3)是定义在 R 6 \mathbb{R^6} R6上上的向量.记作 ξ \xi ξ:

s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{se}(3) = \left\{ \xi = \left[\begin{array}{c} \rho \\ \phi\end{array}\right] \in \mathbb{R^6}, \rho \in \mathbb{R^3}, \phi \in \mathfrak{so}(3), \xi^\wedge = \left[\begin{array}{cc} \phi^\wedge & \rho \\ 0^T & 0\end{array}\right] \in \mathbb{R^{4\times 4}} \right\} se(3)={ ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
s e ( 3 ) \mathfrak{se}(3) se(3)中的每个元素 ξ \xi ξ,是一个六维向量.前三维 ρ \rho ρ表示平移;后三维 ϕ \phi ϕ表示旋转,本质上是 s o ( 3 ) \mathfrak{so}(3) so(3)元素.
在这里同样使用 ∧ ^\wedge ∧符号将六维向量扩展成为四维矩阵,但不再表示反对称

ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 \xi^\wedge = \left[\begin{array}{cc} \phi^\wedge & \rho \\ 0^T & 0\end{array}\right] \in \mathbb{R^{4 \times 4}} ξ∧=[ϕ∧0Tρ0]∈R4×4
李代数 s e ( 3 ) \mathfrak{se}(3) se(3)的李括号和 s o ( 3 ) \mathfrak{so}(3) so(3)类似:
[ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ [\xi_1, \xi_2] = (\xi^\wedge_1 \xi^\wedge_2 - \xi^\wedge_2 \xi^\wedge_1) ^\vee [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨
s e ( 3 ) \mathfrak{se}(3) se(3)和 S E ( 3 ) SE(3) SE(3)间映射关系为
李群 T = exp ⁡ ( ξ ∧ ) 李代数 ξ = ln ⁡ ( T ) ∨ \begin{aligned} 李群T &= \exp(\xi ^\wedge) \\ 李代数\xi &= \ln (T) ^\vee \end{aligned} 李群T李代数ξ=exp(ξ∧)=ln(T)∨

李群与李代数的转换关系:指数映射和对数映射

S O ( 3 ) SO(3) SO(3)和 s o ( 3 ) \mathfrak{so}(3) so(3)间的转换关系

将三维向量 ϕ \phi ϕ分解为其模长 θ \theta θ和方向向量 α \alpha α,即 ϕ = θ α \phi=\theta\alpha ϕ=θα.则从 s o ( 3 ) \mathfrak{so}(3) so(3)到 S O ( 3 ) SO(3) SO(3)的指数映射可表示为:

R = exp ⁡ ( ϕ ) = exp ⁡ ( θ α ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) α α T + sin ⁡ θ α ∧ R = \exp(\phi) = \exp(\theta \alpha ^\wedge) = \cos \theta I + (1-\cos\theta) \alpha \alpha^T + \sin \theta \alpha ^\wedge R=exp(ϕ)=exp(θα∧)=cosθI+(1−cosθ)ααT+sinθα∧

上式即为旋转向量到旋转矩阵的罗德里格斯公式,可见** s o ( 3 ) \mathfrak{so}(3) so(3)本质上是旋转向量组成的空间**.
从 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的对数映射可表示为:
ϕ = ln ⁡ ( R ) ∨ \phi = \ln®^\vee ϕ=ln®∨

实际计算时可以通过迹的性质分别求出转角 θ \theta θ和转轴 α \alpha α
θ = arccos ⁡ t r ( R ) − 1 2 , R α = α \theta = \arccos \frac{tr®-1}{2}, \qquad R \alpha = \alpha θ=arccos2tr®−1,Rα=α

S E ( 3 ) SE(3) SE(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)间的转换关系

从 s e ( 3 ) \mathfrak{se}(3) se(3)到 S E ( 3 ) SE(3) SE(3)的指数映射可表示为:

T = exp ⁡ ( ξ ∧ ) = [ R J ρ 0 T 1 ] T = \exp(\xi ^\wedge) = \left[\begin{array}{cc} R & J\rho \\ 0^T & 1\end{array}\right] T=exp(ξ∧)=[R0TJρ1]

其中
J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) α α T + 1 − cos ⁡ θ θ α ∧ J = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge J=θsinθI+(1−θsinθ)ααT+θ1−cosθα∧

可以看到,平移部分经过指数映射之后,发生了一次以 J J J为系数矩阵的线性变换.
从 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)的对数映射可表示为:
ξ = ln ⁡ ( T ) ∨ \xi = \ln(T)^\vee ξ=ln(T)∨

实际计算时 ϕ \phi ϕ可以由 S O ( 3 ) SO(3) SO(3)到 s o ( 3 ) \mathfrak{so}(3) so(3)的映射得到, ρ \rho ρ可以由 t = J ρ t=J\rho t=Jρ计算得到.

在这里插入图片描述

李代数求导: 引入李代数的一大动机就是方便求导优化

李群乘法与李代数加法的关系

BCH公式及其近似形式
- 很遗憾地,李群乘积和李代数加法并不等价,即:
  R 1 R 2 = exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 1 ∧ ) ≠ exp ⁡ ( ( ϕ 1 + ϕ 2 ) ∧ ) R_1 R_2 = \exp(\phi_1^\wedge) \exp(\phi_1^\wedge) \ne \exp((\phi_1 + \phi_2)^\wedge) R1R2=exp(ϕ1∧)exp(ϕ1∧)=exp((ϕ1+ϕ2)∧)
  
  李群乘积与李代数运算的对应关系由BCH公式给出:
  
  ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . \ln(\exp(A) \exp(B)) = A+B +\frac{1}{2} [A,B] +\frac{1}{12} [A, [A,B]] -\frac{1}{12} [B, [A,B]] + … ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]−121[B,[A,B]]+…
  
  上式中 [ , ] [,] [,]表示李括号运算.
- 当 ϕ 1 \phi_1 ϕ1或 ϕ 2 \phi_2 ϕ2为小量时,可以对BCH公式进行线性近似,得到李群乘积对应的李代数的表达式:
  R 1 ⋅ R 2 对应的李代数 = ln ⁡ ( exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 1 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为小量时 J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为小量时 R_1 \cdot R_2 对应的李代数 = \ln (\exp(\phi_1^\wedge) \exp(\phi_1^\wedge))\vee \approx \left\{ \begin{aligned} J_l(\phi_2)^{-1} \phi_1 + \phi_2 \quad \text{当 $phi\_1$ 为小量时} \\ J_r(\phi_1)^{-1} \phi_2 + \phi_1 \quad \text{当 $phi\_2$ 为小量时} \end{aligned} \right. R1⋅R2对应的李代数=ln(exp(ϕ1∧)exp(ϕ1∧))∨≈{ Jl(ϕ2)−1ϕ1+ϕ2当ϕ1为小量时Jr(ϕ1)−1ϕ2+ϕ1当ϕ2为小量时
  
  其中左乘雅可比矩阵 J l J_l Jl即为从 S E ( 3 ) SE(3) SE(3)到 s e ( 3 ) \mathfrak{se}(3) se(3)对数映射中的雅可比矩阵
  J l = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) α α T + 1 − cos ⁡ θ θ α ∧ J_l = \frac{\sin\theta}{\theta} I + (1-\frac{\sin\theta}{\theta}) \alpha \alpha^T + \frac{1- \cos\theta}{\theta} \alpha^\wedge Jl=θsinθI+(1−θsinθ)ααT+θ1−cosθα∧
  
  其逆为
  J l − 1 = θ 2 cot ⁡ θ 2 I + ( 1 − θ 2 cot ⁡ θ 2 ) α α T + θ 2 α ∧ J_l^{-1} = \frac{\theta}{2} \cot{\frac{\theta}{2}} I + (1-\frac{\theta}{2} \cot{\frac{\theta}{2}}) \alpha \alpha^T + \frac{\theta}{2} \alpha^\wedge Jl−1=2θcot2θI+(1−2θcot2θ)ααT+2θα∧
  
  右乘雅可比矩阵只需对自变量取负号即可
  J r ( ϕ ) = J l ( − ϕ ) J_r(\phi) = J_l(-\phi) Jr(ϕ)=Jl(−ϕ)
李群 S O ( 3 ) SO(3) SO(3)乘法与李代数 s o ( 3 ) \mathfrak{so}(3) so(3)加法的关系:
- 对旋转 R R R(李代数为 ϕ \phi ϕ)左乘一个微小旋转 Δ R \Delta R ΔR(李代数为 Δ ϕ \Delta \phi Δϕ),得到的旋转李群 Δ R ⋅ R \Delta R\cdot R ΔR⋅R对应的李代数为:
  Δ R ⋅ R 对应的李代数 = ln ⁡ ( exp ⁡ ( Δ ϕ ∧ ) exp ⁡ ( ϕ ∧ ) ) = ϕ + J l − 1 ( ϕ ) Δ ϕ \Delta R \cdot R 对应的李代数 = \ln \left( \exp(\Delta \phi^\wedge) \exp(\phi^\wedge) \right) = \phi + J_l^{-1}(\phi)\Delta \phi ΔR⋅R对应的李代数=ln(exp(Δϕ∧)exp(ϕ∧))=ϕ+Jl−1(ϕ)Δϕ
- 反之,李代数加法 ( ϕ + Δ ϕ ) (\phi+\Delta \phi) (ϕ+Δϕ)对应的李群元素可表示为:
  ( ϕ + Δ ϕ ) 对应的李群 = exp ⁡ ( ( ϕ + Δ ϕ ) ∧ ) = exp ⁡ ( ( J l Δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ϕ ∧ ) exp ⁡ ( ( J r Δ ϕ ) ∧ ) (\phi+\Delta \phi)对应的李群 = \exp((\phi+\Delta \phi)^\wedge) = \exp((J_l \Delta \phi)^\wedge) \exp(\phi^\wedge)= \exp(\phi^\wedge) \exp((J_r \Delta \phi)^\wedge) (ϕ+Δϕ)对应的李群=exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
同理,李群 S E ( 3 ) SE(3) SE(3)乘法与李代数 s e ( 3 ) \mathfrak{se}(3) se(3)加法的关系:
exp ⁡ ( Δ ξ ∧ ) exp ⁡ ( ξ ∧ ) ≈ exp ⁡ ( ( J l − 1 Δ ξ + ξ ) ∧ ) exp ⁡ ( ξ ∧ ) exp ⁡ ( Δ ξ ∧ ) ≈ exp ⁡ ( ( J r − 1 Δ ξ + ξ ) ∧ ) \exp(\Delta \xi^\wedge) \exp(\xi^\wedge) \approx \exp\left( (J_l^{-1}\Delta \xi + \xi)^\wedge \right) \\ \exp(\xi^\wedge) \exp(\Delta \xi^\wedge) \approx \exp\left( (J_r^{-1}\Delta \xi + \xi)^\wedge \right) exp(Δξ∧)exp(ξ∧)≈exp((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧)

S O ( 3 ) SO(3) SO(3)上的李代数求导

对空间点 p p p进行旋转,得到 R p Rp Rp,旋转之后点的坐标对旋转的导数可表示为:
∂ ( R p ) ∂ R \frac{\partial(Rp)}{\partial R} ∂R∂(Rp)

对于上式的求导,有两种方式:

用李代数 ϕ \phi ϕ表示姿态 R R R,然后根据李代数加法对 ϕ \phi ϕ求导.
用李代数 φ \varphi φ表示微小扰动 ∂ R \partial R ∂R,然后根据李群左乘对 φ \varphi φ求导.

其中扰动模型表达式简单,更为实用.

李代数求导

用李代数 ϕ \phi ϕ表示姿态 R R R,求导得到
∂ ( R p ) ∂ R = ∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ = − ( R p ) ∧ J l \frac{\partial(Rp)}{\partial R} = \frac{\partial( \exp(\phi^\wedge) p)}{\partial \phi} = -(Rp) ^\wedge J_l ∂R∂(Rp)=∂ϕ∂(exp(ϕ∧)p)=−(Rp)∧Jl

扰动模型(左乘)

另一种求导方式是对 R R R进行一次左乘扰动 ∂ R \partial R ∂R,设左乘扰动 ∂ R \partial R ∂R对应的李代数为 φ \varphi φ,对 φ \varphi φ求导,得到
∂ ( R p ) ∂ R = exp ⁡ ( ( ϕ + φ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = − ( R p ) ∧ \frac{\partial(Rp)}{\partial R} = \frac{ \exp((\phi+\varphi)^\wedge)p - \exp(\phi^\wedge)p }{\varphi} =-(Rp) ^\wedge ∂R∂(Rp)=φexp((ϕ+φ)∧)p−exp(ϕ∧)p=−(Rp)∧

S E ( 3 ) SE(3) SE(3)上的李代数求导

类似地,空间点 p p p经过变换 T T T得到 T p Tp Tp,给 T T T左乘一个扰动 Δ T = exp ⁡ ( δ ξ ∧ ) \Delta T = \exp (\delta \xi ^\wedge) ΔT=exp(δξ∧),则有
∂ ( R p ) δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] = ( T P ) ⊙ \frac{\partial(Rp)}{\delta \xi} = \left[\begin{array}{cc} I & -(Rp+t)^\wedge \\ 0^T & 0^T\end{array}\right]= (TP) ^ \odot δξ∂(Rp)=[I0T−(Rp+t)∧0T]=(TP)⊙

ch05 相机与图像

针孔相机模型

在这里插入图片描述

O − x − y − z O-x-y-z O−x−y−z为相机坐标系,现实空间点 P P P的相机坐标为 [ X , Y , Z ] T [X,Y,Z]^T [X,Y,Z]T,投影到 O ′ − x ′ − y ′ O’-x’-y’ O′−x′−y′平面上的点 P ′ P’ P′,坐标为 [ X ′ , Y ′ , Z ′ ] T [X’,Y’,Z’]^T [X′,Y′,Z′]T.

将成像平面对称到相机前方,根据几何相似关系 Z f = X X ′ = Y Y ′ \frac{Z}{f} = \frac{X}{X’} = \frac{Y}{Y’} fZ=X′X=Y′Y,整理得到投影点 P ′ P’ P′在投影平面上的坐标 P ′ = [ X ′ , Y ′ ] P’=[X’,Y’] P′=[X′,Y′]:

{ X ′ = f X Z Y ′ = f Y Z \left\{ \begin{aligned} X’ = f \frac{X}{Z} \\ Y’ = f \frac{Y}{Z} \\ \end{aligned} \right. ⎩⎪⎪⎨⎪⎪⎧X′=fZXY′=fZY
转换得到投影点 P ′ P’ P′在像素平面上的像素坐标 P u , v = [ u , v ] T P_{u,v} = [u, v]^T Pu,v=[u,v]T
{ u = α X ′ + c x = f x X Z + c x v = β Y ′ + c y = f x X Z + c x \left\{ \begin{aligned} u = \alpha X’ + c_x &= f_x \frac{X}{Z}+c_x \\ v = \beta Y’ + c_y &= f_x \frac{X}{Z}+c_x \\ \end{aligned} \right. ⎩⎪⎪⎨⎪⎪⎧u=αX′+cxv=βY′+cy=fxZX+cx=fxZX+cx

上式中 u u u, v v v, c x c_x cx, c y c_y cy, f x f_x fx, f y f_y fy的单位为像素, α \alpha α, β \beta β的单位为像素/米.
将上式写成矩阵形式,得到**现实空间点相机坐标 P P P和投影点像素坐标 P u v P_{uv} Puv**之间的关系:
Z P u v = Z [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X Y Z ] ≜ K P Z P_{uv} = Z \left[\begin{array}{c} u \\ v \\ 1 \end{array}\right] = \left[\begin{array}{ccc} f_x &0 &c_x \\ 0 &f_y &c_y \\ 0 &0 &1 \end{array}\right] \left[\begin{array}{c} X \\ Y \\ Z \end{array}\right] \triangleq KP ZPuv=Z⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XYZ⎦⎤≜KP

其中矩阵 K K K称为相机的内参数矩阵.
上式中的 P P P为现实空间点在相机坐标系下的相机坐标,将其转为世界坐标 P W P_W PW,有

Z P u v = K ( R P W + t ) = K T P W ZP_{uv} = K(RP_W+t)= KTP_W ZPuv=K(RPW+t)=KTPW

因此 R R R, t t t(或 T T T)又称为相机的外参数.
将最后一维进行归一化处理,得到点 P P P在归一化平面的归一化坐标 P c = [ X / Z , Y / Z , 1 ] T P_c=[X/Z, Y/Z, 1]^T Pc=[X/Z,Y/Z,1]T

P c = P Z = K − 1 P u v P_c = \frac{P}{Z}={K^{-1} P_{uv}} Pc=ZP=K−1Puv

在这里插入图片描述

参数矩阵有内参数 K K K和外参数 R R R, t t t,其中:

内参数矩阵 K K K体现了归一化相机坐标到像素坐标的变换.

之所以是归一化坐标,这体现了投影性质:在某一条直线上的空间点,最终会投影到同一像素点上.
外参数矩阵 R R R, t t t(或 T T T)体现了世界坐标到相机坐标的变换.

畸变模型

畸变包含两种: 径向畸变和切向畸变.

径向畸变: 由透镜形状引起,主要包括桶形畸变和枕形畸变.

可以看成坐标点沿着长度方向发生了变化,也就是其距离原点的长度发生了变化.
x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) x_{distorted} = x(1+ k_1r^2 + k_2r^4 + k_3r6) \\ y_{distorted} = y(1+ k_1r^2 + k_2r^4 + k_3r6) xdistorted=x(1+k1r2+k2r4+k3r6)ydistorted=y(1+k1r2+k2r4+k3r6)
切向畸变: 由透镜和成像平面不严格平行引起.

可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化.
x d i s t o r t e d = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y x_{distorted} = x + 2p_1xy + p_2(r^2+2x2) \\ y_{distorted} = y + p_1(r^2+2y2) + 2p_2xy xdistorted=x+2p1xy+p2(r2+2x2)ydistorted=y+p1(r2+2y2)+2p2xy

单目相机的成像过程

单目相机的成像过程：

世界坐标系下有一个固定的原点 P P P,其世界坐标 P W P_W PW
由于相机在运动,它的运动由 R R R, t t t或变换矩阵 T ∈ S E ( 3 ) T\in SE(3) T∈SE(3)描述.原点 P P P的相机坐标 P c ~ = R P W + t \tilde{P_c}=RP_W+t Pc~=RPW+t
这时 P c ~ \tilde{P_c} Pc~的分量为 X X X, Y Y Y, Z Z Z,把它们投影到归一化平面 Z = 1 Z =1 Z=1上,得到 P P P的归一化相机坐标 P c = P c ~ Z = [ X Z , Y Z , 1 ] T P_c =\frac{\tilde{P_c}}{Z}=[\frac{X}{Z},\frac{Y}{Z}, 1] ^T Pc=ZPc~=[ZX,ZY,1]T
有畸变时,根据畸变参数计算 P c P_c Pc发生畸变后的归一化相机坐标
P P P的归一化相机坐标 P c P_c Pc经过内参 K K K后,对应到它的像素坐标 P u v = K P c P_{uv}=KP_c Puv=KPc

在讨论相机成像模型时,我们一共谈到了四种坐标: 世界坐标、相机坐标、归一化相机坐标和像素坐标.请读者厘清它们的关系,它反映了整个成像的过程.

ch06 非线性优化

状态估计问题

最大后验与最大似然

SLAM模型由状态方程和运动方程构成:
{ x k = f ( x k − 1 , u k , w k ) z k , j = h ( y j , x k , v k , j ) \left\{ \begin{aligned} x_k &= f(x_{k-1}, u_k, w_k) \\ z_{k,j} &= h(y_j, x_k, v_{k,j}) \end{aligned} \right. { xkzk,j=f(xk−1,uk,wk)=h(yj,xk,vk,j)

通常假设两个噪声项 w k w_k wk, v k , j v_{k,j} vk,j满足零均值的高斯分布:
w k ∼ N ( 0 , R k ) , v k , j ∼ N ( 0 , Q k , j ) w_k \sim \mathcal{N}(0, R_k) ,\; v_{k,j} \sim \mathcal{N}(0, Q_{k,j}) wk∼N(0,Rk),vk,j∼N(0,Qk,j)

对机器人的估计,本质上就是已知输入数据 u u u和观测数据 z z z的条件下,求机器人位姿 x x x和路标点 y y y的条件概率分布:
P ( x , y ∣ z , u ) P(x,y|z,u) P(x,y∣z,u)

利用贝叶斯法则,有:
P ( x , y ∣ z , u ) = P ( z , u ∣ x , y ) P ( x , y ) P ( z , u ) ∝ P ( z , u ∣ x , y ) P ( x , y ) P(x,y|z,u) = \frac{P(z,u|x,y) P(x,y)}{P(z,u)} \propto P(z,u|x,y) P(x,y) P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝P(z,u∣x,y)P(x,y)

其中 P ( x , y ∣ z , u ) P(x,y|z,u) P(x,y∣z,u)为后验概率, P ( z , u ∣ x , y ) P(z,u|x,y) P(z,u∣x,y)为似然, P ( x , y ) P(x,y) P(x,y)为先验,上式可表述为后验概率 ∝ 似然 ⋅ 先验后验概率 \propto 似然 \cdot 先验后验概率∝似然⋅先验.直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下后验概率最大化则是可行的:
( x , y ) M A P ∗ = arg ⁡ max ⁡ P ( x , y ∣ z , u ) = arg ⁡ max ⁡ P ( z , u ∣ x , y ) P ( x , y ) (x,y)^*_{MAP} = \arg \max P(x,y | z,u) = \arg \max P(z,u|x,y) P(x,y) (x,y)MAP∗=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)

求解最大后验概率相当于最大化似然和先验的乘积.因为 x x x, y y y未知,即不知道先验,则可以求最大似然估计:
( x , y ) M L E ∗ = arg ⁡ max ⁡ P ( z , u ∣ x , y ) (x,y)^*_{MLE} = \arg \max P(z,u|x,y) (x,y)MLE∗=argmaxP(z,u∣x,y)

最大似然估计的直观意义为:在什么样的状态下，最可能产生现在观测到的数据.

最小二乘

基于观测数据 z z z的最小二乘

对于某一次观测
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

由于假设噪声 v k , j ∼ N ( 0 , Q k , j ) v_{k,j} \sim \mathcal{N}(0, Q_{k,j}) vk,j∼N(0,Qk,j),则观测数据 z j , k z_{j,k} zj,k的似然为
P ( z j , k ∣ x k , y j ) = N ( h ( y j , x k ) , Q k , j ) P(z_{j,k}|x_k,y_j) = \mathcal{N} (h(y_j, x_k), Q_{k,j}) P(zj,k∣xk,yj)=N(h(yj,xk),Qk,j)

将上式代入高斯分布表达式中,并取负对数,得到
( x k , y j ) ∗ = arg ⁡ max ⁡ N ( h ( y j , x k ) , Q k , j ) = arg ⁡ min ⁡ ( ( z k , j − h ( x k , y j ) ) T Q k , j − 1 ( z k , j − h ( x k , y j ) ) ) \begin{aligned} (x_k,y_j)^* &= \arg \max \mathcal{N} (h(y_j, x_k), Q_{k,j}) \\ &= \arg \min \left( (z_{k,j} - h(x_k, y_j))^T Q_{k,j}^{-1} (z_{k,j} - h(x_k, y_j)) \right) \end{aligned} (xk,yj)∗=argmaxN(h(yj,xk),Qk,j)=argmin((zk,j−h(xk,yj))TQk,j−1(zk,j−h(xk,yj)))

上式等价于最小化噪声项(即误差)的一个二次型,其中 Q k , j − 1 Q_{k,j}^{-1} Qk,j−1称为信息矩阵,即高斯分布协方差矩阵的逆.

基于观测数据 z z z和输入数据 u u u的最小二乘

因为观测 z z z和输入 u u u是独立的,因此可对 z z z和 u u u的联合似然进行因式分解:
P ( x , y ∣ z , u ) = ∏ k P ( u k ∣ x k − 1 , x k ) ∏ k , j P ( z j , k ∣ x k , y j ) P(x,y|z,u) = \prod_k P(u_k|x_{k-1},x_k) \prod_{k,j} P(z_{j,k}|x_k,y_j) P(x,y∣z,u)=k∏P(uk∣xk−1,xk)k,j∏P(zj,k∣xk,yj)

定义输入和观测数据与模型之间的误差:
e u , k = x k − f ( x k − 1 , u k ) e z , j , k = z k , j − h ( x k , y j ) \begin{aligned} e_{u,k} &= x_{k} - f(x_{k-1}, u_k) \\ e_{z,j,k} &= z_{k,j} - h(x_k,y_j) \end{aligned} eu,kez,j,k=xk−f(xk−1,uk)=zk,j−h(xk,yj)

定义
J ( x , y ) = ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , k , j T Q k , j − 1 e z , k , j J(x,y) = \sum_k e_{u,k}^T R_k^{-1}e_{u,k} + \sum_k \sum_j e_{z,k,j}^T Q_{k,j}^{-1}e_{z,k,j} J(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,k,jTQk,j−1ez,k,j

则有
( x k , y j ) ∗ = arg ⁡ min ⁡ J ( x , y ) (x_k,y_j)^* = \arg \min J(x,y) (xk,yj)∗=argminJ(x,y)

非线性最小二乘

对于非线性最小二乘问题:
min ⁡ x F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 \min_{x} F(x) = \frac{1}{2} ||f(x)||_2^2 xminF(x)=21∣∣f(x)∣∣22

求解该问题的具体步骤如下:

给定某个初始值 x 0 x_0 x0
对于第 k k k次迭代,寻找一个增量 Δ x k \Delta x_k Δxk,使得 ∣ ∣ F ( x k + Δ x k ) ∣ ∣ 2 2 ||F(x_k +\Delta x_k)||_2^2 ∣∣F(xk+Δxk)∣∣22达到极小值
若 Δ x k \Delta x_k Δxk足够小,则停止
否则,令 x k + 1 = x k + Δ x k x_{k +1} =x_k +\Delta x_k xk+1=xk+Δxk,返回第2步

这样,最小二乘问题被转化为一个不断寻找下降增量 Δ x k \Delta x_k Δxk的问题.,具体有以下方法

一阶和二阶梯度法

将目标函数 F ( x ) F(x) F(x)在 x k x_k xk附近进行泰勒展开
F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) x k F(x_k +\Delta x_k) \approx F(x_k) + J(x_k)^T \Delta x_k + \frac{1}{2} \Delta x_k^T H(x_k) x_k F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)xk

其中 J ( x ) J(x) J(x)是 F ( x ) F(x) F(x)关于 x x x的一阶导数矩阵, H ( x ) H(x) H(x)是 F ( x ) F(x) F(x)关于 x x x的二阶导数矩阵.

若 Δ x k \Delta x_k Δxk取一阶导数,则
Δ x k ∗ = − J ( x k ) \Delta x_k^* = -J(x_k) Δxk∗=−J(xk)
若 Δ x k \Delta x_k Δxk取二阶导数,则
Δ x k ∗ = arg ⁡ min ⁡ ( F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) x k ) \Delta x_k^* = \arg \min \left( F(x_k) + J(x_k)^T \Delta x_k + \frac{1}{2} \Delta x_k^T H(x_k) x_k \right) Δxk∗=argmin(F(xk)+J(xk)TΔxk+21ΔxkTH(xk)xk)

令上式对 Δ x k \Delta x_k Δxk导数等于0,则 Δ x k ∗ \Delta x_k^* Δxk∗可以取 H Δ x k = − J H \Delta x_k = -J HΔxk=−J的解.

高斯牛顿法

将 f ( x k ) f(x_k) f(xk)而非 F ( x k ) F(x_k) F(xk)在 x k x_k xk附近进行泰勒展开
f ( x k + Δ x k ) ≈ f ( x k ) + J ( x k ) T Δ x k f(x_k+\Delta x_k) \approx f(x_k) + J(x_k)^T \Delta x_k f(xk+Δxk)≈f(xk)+J(xk)TΔxk

则
Δ x k ∗ = arg ⁡ min ⁡ Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) T Δ x k ∣ ∣ 2 \Delta x_k^* = \arg \min_{\Delta x_k} \frac{1}{2} ||f(x_k)+J(x_k)^T \Delta x_k||^2 Δxk∗=argΔxkmin21∣∣f(xk)+J(xk)