ch02 初识SLAM

经典视觉SLAM框架

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

1. 传感器信息读取: 在视觉SLAM中主要为相机图像信息的读取和预处理.如果是在机器人中,还可能有码盘、惯性传感器等信息的读取和同步.

2. 视觉里程计(Visual Odometry,VO): 视觉里程计的任务是估算相邻图像间相机的运动,以及局部地图的样子.VO又称为前端(Front End).

   视觉里程计不可避免地会出现累积漂移(Accumulating Drift)问题.

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

   在视觉 SLAM中,前端和计算机视觉研究领域更为相关,比如图像的特征提取与匹配等,后端则主要是滤波与非线性优化算法.

4. 回环检测 (Loop Closing): 回环检测判断机器人是否到达过先前的位置.如果检测到回环,它会把信息提供给后端进行处理.

5. 建图 (Mapping): 它根据估计的轨迹,建立与任务要求对应的地图.

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

SLAM问题的数学表述

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

1. 什么是运动 ?我们要考虑从$k-1$时刻到$k$时刻,小萝卜的位置$x$是如何变化的.

   运动方程:

   $$x_k=f(x_{k-1},u_k,w_k)$$

   • $x_k,x_{k-1}$表示小萝卜在$k$和$k-1$时刻的位置
   • $u_k$表示运动传感器的读数(有时也叫输入)
   • $w_k$表示噪声

2. 什么是观测 ?假设小萝卜在$k$时刻于$x_k$处探测到了某一个路标$y_j$,我们要考虑这件事情是如何用数学语言来描述的.

   观测方程:

   $$z_{k,j}=h(y_j,x_k,v_{k,j})$$

   • $z_{k,j}$表示小萝卜在$x_k$位置上看到路标点$y_j$,产生的观测数据
   • $y_j$表示第$j$个路标点
   • $v_{k,j}$表示噪声

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

ch03 三维空间刚体运动

旋转矩阵

点和向量,坐标系

1. 向量$a$在线性空间的基$[e_1,e_2,e_3]$下的坐标为$[a_1,a_2,a_3{]}^T$.

   $$a=[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$

2. 向量的内积与外积

   • 向量的内积: 描述向量间的投影关系
     $$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|\cos ⟨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^{\wedge }$表示$a$的反对称矩阵
     $$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

坐标系间的欧氏变换

1. 欧式变换:

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

   一个欧式变换由一个旋转和一个平移组成.

2. 旋转矩阵$R$:

   • 旋转矩阵$R$的推导:

     设单位正交基$[e_1,e_2,e_3]$经过一次旋转变成了$[e_1^{\prime },e_2^{\prime },e_3^{\prime }]$,对于同一个向量$a$,在两个坐标系下的坐标分别为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$.根据坐标的定义:
     $$[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{\prime },e_2^{\prime },e_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$

     等式左右两边同时左乘$[e_1^T,e_2^T,e_3^T{]}^T$,得到
     $$\left[\begin{array}{c}{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 }$$

     矩阵$R$描述了旋转,称为旋转矩阵.

   • 旋转矩阵$R$的性质

     1. 旋转矩阵是行列式为1的正交矩阵,任何行列式为1的正交矩阵也是一个旋转矩阵.所有旋转矩阵构成特殊正交群$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(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

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

3. 欧式变换的向量表示:

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

变换矩阵与齐次坐标

1. 变换矩阵$T$:

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

   $$\left[\begin{array}{c}{a}^{\prime }\\ 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]$$
   齐次坐标的意义在于将欧式变换表示为线性关系.

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

   1. 变换矩阵$T$构成特殊欧式群$SE$
      $$SE(3)=\left\{T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\right\}$$

   2. 变换矩阵的逆表示一个反向的欧式变换
      $$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0& 1\end{array}\right]$$

齐次坐标(Homogeneous Coordinate)的优势

优势1:方便判断是否在直线或平面上

若点$p=(x,y)$在直线$l=(a,b,c)$上,则有:
$$ax+by+c=[a,b,c{]}^T\cdot [x,y,1]=l^T\cdot p^{\prime }=0$$

若点$p=(x,y,z)$在平面$A=(a,b,c,d)$上,则有:
$$ax+by+cz+d=[a,b,c,d{]}^T\cdot [x,y,z,1]=A^T\cdot p^{\prime }=0$$

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

在齐次坐标下,

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

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

• 性质1的证明:
  $$\begin{gathered} l^T\cdot p=(p\times q)\cdot p=0 \\ {l}^T\cdot q=(p\times q)\cdot q=0 \end{gathered}$$

• 性质2的证明:
  $$\begin{gathered} l^T\cdot p=l^T\cdot (l\times m)=0 \\ {m}^T\cdot p=m^T\cdot (l\times m)=0 \end{gathered}$$

优势3:能够区分向量和点

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

优势4:能够表达无穷远点

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

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

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

变换形式	图形示意	数学变换	MATLAB函数
位移(Translation)		$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& t_x\\ 0& 1& t_y\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$	imtranslate()
缩放(Scale)		$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}{s}_x& 0& 0\\ 0& s_y& 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$	imresize()
错切(Shear)		$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}1& h_x& 0\\ h_y& 1& 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$
旋转(Rotate)		$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$	imrotate()

旋转向量和欧拉角

旋转向量

• 旋转矩阵的缺点:

  1. 旋转矩阵有9个量,但一次旋转只有3个自由度,这种表达方式是冗余的.
  2. 旋转矩阵自带约束(必须是行列式为1的正交矩阵),这些约束会给估计和优化带来困难.

• 旋转向量: 任意旋转都可以用一个旋转轴和一个旋转角 来刻画.于是,我们可以使用一个向量,其方向表示旋转轴而长度表示旋转角.这种向量称为旋转向量(或轴角,Axis-Angle).

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

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

  设旋转向量$R$表示一个绕单位向量$n$,角度为$\theta$的旋转.

  • 旋转向量到旋转矩阵:
    $$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$$

  • 旋转矩阵到旋转向量:

    • 旋转角$\theta =\arccos \left(\frac{tr(R)-1}{2}\right)$
    • 旋转轴$n$是矩阵$R$特征值1对应的特征向量

欧拉角

• 欧拉角将一次旋转分解成3个分离的转角.常用的一种ZYX转角将任意旋转分解成以下3个轴上的转角:

  1. 绕物体的$Z$轴旋转,得到偏航角yaw
  2. 绕旋转之后的$Y$轴旋转,得到俯仰角pitch
  3. 绕旋转之后的$X$轴旋转,得到滚转角roll

• 欧拉角的一个重大缺点是万向锁问题(奇异性问题): 在俯仰角为$\pm$90° 时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由3次旋转变成了2次旋转).

四元数

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

四元数的定义

1. 四元数的定义

   一个四元数$q$拥有一个实部和三个虚部
   $$q=q_0+q_{1}i+q_{2}j+q_{3}k$$

   其中$i$,$j$,$k$,为四元数的3个虚部,它们满足以下关系式(自己和自己的运算像复数,自己和别人的运算像叉乘):
   $$\{\begin{array}{rl}& i^2=j^2=k^2=-1\\ & ij=k,ji=-k\\ & jk=i,kj=-i\\ & ki=j,ik=-j\end{array}$$

   也可以用一个标量和一个向量来表达四元数:
   $$q=[s,v],s=q_0\in \mathbb{R}v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$

   $s$为四元数的实部,$v$为四元数的虚部.有实四元数和虚四元数的概念.

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

   • 在二维情况下,任意一个旋转都可以用单位复数来描述,乘$i$就是绕$i$轴旋转90°.
   • 在三维情况下,任意一个旋转都可以用单位四元数来描述,乘$i$就是绕$i$轴旋转180°.

3. 单位四元数和旋转向量之间的转换:

   设单位四元数$q$表示一个绕单位向量$n=[n_x,n_y,n_z{]}^T$,角度为$\theta$的旋转.

   • 从旋转向量到单位四元数:

   $$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$$

   • 从单位四元数到旋转向量:
     $$\{\begin{array}{rl}& \theta =2\arccos q_0\\ & [n_x,n_y,n_z]=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}$$

用单位四元数表示旋转

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

1. 把三维空间点用一个虚四元数$p$表示:
   $$p=[0,x,y,z]=[0,v]$$

2. 把旋转用单位四元数$q$表示:
   $$q=[\cos \frac{\theta }{2},n\sin \frac{\theta }{2}]$$

3. 旋转后的点$p^{\prime }$可表示为:
   $$p^{\prime }=qp{q}^{-1}$$

这样得到的点$p^{\prime }$仍为一个纯虚四元数,其虚部的3个分量表示旋转后3D点的坐标.

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

ch04 李群与李代数

李群与李代数基础

旋转矩阵构成特殊正交群$SO(3)$,变换矩阵构成了特殊欧氏群$SE(3)$.
$$\begin{array}{rl}SO(3)& =\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^T=I,\det (R)=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{array}$$

群的定义

• 群(Group)是一种集合加上一种运算的代数结构.把集合记作$A$,运算记作$\cdot$ ,那么群可以记作$G=(A,\cdot )$.群要求这个运算满足如下条件(封结幺逆):

  1. 封闭性: $\forall a_1,a_2\in A,a_1\cdot a_2\in A$.
  2. 结合律: $\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
  3. 幺元: $\exists a_0\in A,\mathrm{s}.\mathrm{t}.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
  4. 逆: $\forall a\in A,\exists a^{-1}\in A,\mathrm{s}.\mathrm{t}.a\cdot a^{-1}=a_0$

• 李群是指具有连续(光滑)性质的群.$SO(3)$和$SE(3)$都是李群

李代数的定义

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

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

1. 封闭性: $\forall X,Y\in V,[X,Y]\in V$.

2. 双线性: $\forall X,Y,Z \in V, a,b \in F $有:
   $$[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$$

3. 自反性: $\forall X,\in V,[X,X]=0$.

4. 雅可比等价$\forall X,Y,Z\in V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$.

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

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

• 李群$SO(3)$对应的李代数$\mathfrak{s}\mathfrak{o}(3)$是定义在${\mathbb{R}}^3$上的向量,记作$\varphi$.

  $$\mathfrak{s}\mathfrak{o}(3)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}\right\}$$

• 李代数$\mathfrak{s}\mathfrak{o}(3)$的李括号为
  $$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$
  其中${}^{\vee }$是${}^{\wedge }$的逆运算,表示将反对称矩阵还原为向量

• $\mathfrak{s}\mathfrak{o}(3)$和$SO(3)$间的映射关系为

  $$\begin{array}{rl}李群R& =\exp ({\varphi }^{\wedge })=\exp (\Phi )\\ 李代数\varphi & =\ln (R{)}^{\vee }\end{array}$$

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

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

  $$\mathfrak{s}\mathfrak{e}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{s}\mathfrak{o}(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
  $\mathfrak{s}\mathfrak{e}(3)$中的每个元素$\xi$,是一个六维向量.前三维$\rho$表示平移;后三维$\varphi$表示旋转,本质上是$\mathfrak{s}\mathfrak{o}(3)$元素.

• 在这里同样使用${}^{\wedge }$符号将六维向量扩展成为四维矩阵,但不再表示反对称

  $${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$

• 李代数$\mathfrak{s}\mathfrak{e}(3)$的李括号和$\mathfrak{s}\mathfrak{o}(3)$类似:
  $$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$

• $\mathfrak{s}\mathfrak{e}(3)$和$SE(3)$间映射关系为
  $$\begin{array}{rl}李群T& =\exp ({\xi }^{\wedge })\\ 李代数\xi & =\ln (T{)}^{\vee }\end{array}$$

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

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

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

  $$R=\exp (\varphi )=\exp (\theta {\alpha }^{\wedge })=\cos \theta I+(1-\cos \theta )\alpha {\alpha }^T+\sin \theta {\alpha }^{\wedge }$$

  上式即为旋转向量到旋转矩阵的罗德里格斯公式,可见**$\mathfrak{s}\mathfrak{o}(3)$本质上是旋转向量组成的空间**.

• 从$SO(3)$到$\mathfrak{s}\mathfrak{o}(3)$的对数映射可表示为:
  $$\varphi =\ln (R{)}^{\vee }$$

  实际计算时可以通过迹的性质分别求出转角$\theta$和转轴$\alpha$
  $$\theta =\arccos \frac{tr(R)-1}{2},R\alpha =\alpha$$

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

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

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

  其中
  $$J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\alpha {\alpha }^T+\frac{1-\cos \theta }{\theta }{\alpha }^{\wedge }$$

  可以看到,平移部分经过指数映射之后,发生了一次以$J$为系数矩阵的线性变换.

• 从$SE(3)$到$\mathfrak{s}\mathfrak{e}(3)$的对数映射可表示为:
  $$\xi =\ln (T{)}^{\vee }$$

  实际计算时$\varphi$可以由$SO(3)$到$\mathfrak{s}\mathfrak{o}(3)$的映射得到,$\rho$可以由$t=J\rho$计算得到.

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

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

1. BCH公式及其近似形式

   • 很遗憾地,李群乘积和李代数加法并不等价,即:
     $$R_1{R}_2=\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_1^{\wedge })\ne \exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })$$

     李群乘积与李代数运算的对应关系由BCH公式给出:

     $$\ln (\exp (A)\exp (B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...$$

     上式中$[,]$表示李括号运算.

   • 当${\varphi }_1$或${\varphi }_2$为小量时,可以对BCH公式进行线性近似,得到李群乘积对应的李代数的表达式:
     $$R_1\cdot R_2对应的李代数=\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_1^{\wedge }){)}^{\vee }\approx \{\begin{array}{r}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2\text{当ϕ1为小量时}\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1\text{当ϕ2为小量时}\end{array}$$

     其中左乘雅可比矩阵$J_l$即为从$SE(3)$到$\mathfrak{s}\mathfrak{e}(3)$对数映射中的雅可比矩阵
     $$J_l=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })\alpha {\alpha }^T+\frac{1-\cos \theta }{\theta }{\alpha }^{\wedge }$$

     其逆为
     $$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 }$$

     右乘雅可比矩阵只需对自变量取负号即可
     $$J_r(\varphi )=J_l(-\varphi )$$

2. 李群$SO(3)$乘法与李代数$\mathfrak{s}\mathfrak{o}(3)$加法的关系:

   • 对旋转$R$(李代数为$\varphi$)左乘一个微小旋转$\Delta R$(李代数为$\Delta \varphi$),得到的旋转李群$\Delta R\cdot R$对应的李代数为:
     $$\Delta R\cdot R对应的李代数=\ln \left(\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })\right)=\varphi +J_l^{-1}(\varphi )\Delta \varphi$$

   • 反之,李代数加法$(\varphi +\Delta \varphi )$对应的李群元素可表示为:
     $$(\varphi +\Delta \varphi )对应的李群=\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ((J_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })$$

3. 同理,李群$SE(3)$乘法与李代数$\mathfrak{s}\mathfrak{e}(3)$加法的关系:
   $$\begin{gathered} \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) \end{gathered}$$

$SO(3)$上的李代数求导

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

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

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

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

李代数求导

用李代数$\varphi$表示姿态$R$,求导得到
$$\frac{\partial (Rp)}{\partial R}=\frac{\partial (\exp ({\varphi }^{\wedge })p)}{\partial \varphi }=-(Rp{)}^{\wedge }{J}_l$$

扰动模型(左乘)

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

$SE(3)$上的李代数求导

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

ch05 相机与图像

针孔相机模型

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

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

  $$\{\begin{array}{r}{X}^{\prime }=f\frac{X}{Z}\\ Y^{\prime }=f\frac{Y}{Z}\end{array}$$

• 转换得到投影点$P^{\prime }$在像素平面上的像素坐标$P_{u,v}=[u,v{]}^T$
  $$\{\begin{array}{rl}u=\alpha X^{\prime }+c_x& =f_x\frac{X}{Z}+c_x\\ v=\beta Y^{\prime }+c_y& =f_x\frac{X}{Z}+c_x\end{array}$$

  上式中$u$,$v$,$c_x$,$c_y$,$f_x$,$f_y$的单位为像素,$\alpha$,$\beta$的单位为像素/米.

• 将上式写成矩阵形式,得到**现实空间点相机坐标$P$和投影点像素坐标$P_{uv}$**之间的关系:
  $$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$$

  其中矩阵$K$称为相机的内参数矩阵.

• 上式中的$P$为现实空间点在相机坐标系下的相机坐标,将其转为世界坐标$P_W$,有

  $$Z{P}_{uv}=K(R{P}_W+t)=KT{P}_W$$

  因此$R$,$t$(或$T$)又称为相机的外参数.

• 将最后一维进行归一化处理,得到点$P$在归一化平面的归一化坐标$P_c=[X/Z,Y/Z,1{]}^T$

  $$P_c=\frac{P}{Z}=K^{-1}{P}_{uv}$$

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

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

   之所以是归一化坐标,这体现了投影性质:在某一条直线上的空间点,最终会投影到同一像素点上.

2. 外参数矩阵$R$,$t$(或$T$)体现了世界坐标到相机坐标的变换.

畸变模型

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

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

  可以看成坐标点沿着长度方向发生了变化,也就是其距离原点的长度发生了变化.
  $$\begin{gathered} x_{distorted}=x(1+k_1{r}^2+k_2{r}^4+k_{3}r6) \\ {y}_{distorted}=y(1+k_1{r}^2+k_2{r}^4+k_{3}r6) \end{gathered}$$

• 切向畸变: 由透镜和成像平面不严格平行引起.

  可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化.
  $$\begin{gathered} x_{distorted}=x+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{distorted}=y+p_1(r^2+2y^2)+2p_{2}xy \end{gathered}$$

单目相机的成像过程

单目相机的成像过程：

1. 世界坐标系下有一个固定的原点$P$,其世界坐标$P_W$
2. 由于相机在运动,它的运动由$R$,$t$或变换矩阵$T\in SE(3)$描述.原点$P$的相机坐标$\widetilde{P_c}=R{P}_W+t$
3. 这时$\widetilde{P_c}$的分量为$X$,$Y$,$Z$,把它们投影到归一化平面$Z=1$上,得到$P$的归一化相机坐标$P_c=\frac{\widetilde{P_c}}{Z}=[\frac{X}{Z},\frac{Y}{Z},1{]}^T$
4. 有畸变时,根据畸变参数计算$P_c$发生畸变后的归一化相机坐标
5. $P$的归一化相机坐标$P_c$经过内参$K$后,对应到它的像素坐标$P_{uv}=K{P}_c$

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

ch06 非线性优化

状态估计问题

最大后验与最大似然

SLAM模型由状态方程和运动方程构成:
$$\{\begin{array}{rl}{x}_k& =f(x_{k-1},u_k,w_k)\\ z_{k,j}& =h(y_j,x_k,v_{k,j})\end{array}$$

通常假设两个噪声项$w_k$,$v_{k,j}$满足零均值的高斯分布:
$$w_k\sim \mathcal{N}(0,R_k),v_{k,j}\sim \mathcal{N}(0,Q_{k,j})$$

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

利用贝叶斯法则,有:
$$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|x,y)$为似然,$P(x,y)$为先验,上式可表述为$后验概率\propto 似然\cdot 先验$.直接求后验分布是困难的,但是求一个状态最优估计,使得在该状态下后验概率最大化则是可行的:
$$(x,y{)}_{MAP}^{\ast }=\arg \max P(x,y|z,u)=\arg \max P(z,u|x,y)P(x,y)$$

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

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

最小二乘

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

对于某一次观测
$$z_{k,j}=h(y_j,x_k)+v_{k,j}$$

由于假设噪声$v_{k,j}\sim \mathcal{N}(0,Q_{k,j})$,则观测数据$z_{j,k}$的似然为
$$P(z_{j,k}|x_k,y_j)=\mathcal{N}(h(y_j,x_k),Q_{k,j})$$

将上式代入高斯分布表达式中,并取负对数,得到
$$\begin{array}{rl}(x_k,y_j{)}^{\ast }& =\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{array}$$

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

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

因为观测$z$和输入$u$是独立的,因此可对$z$和$u$的联合似然进行因式分解:
$$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)$$

定义输入和观测数据与模型之间的误差:
$$\begin{array}{rl}{e}_{u,k}& =x_k-f(x_{k-1},u_k)\\ e_{z,j,k}& =z_{k,j}-h(x_k,y_j)\end{array}$$

定义
$$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}$$

则有
$$(x_k,y_j{)}^{\ast }=\arg \min J(x,y)$$

非线性最小二乘

对于非线性最小二乘问题:
$$\underset{x}{\min }F(x)=\frac{1}{2}||f(x)|{|}_2^2$$

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

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

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

一阶和二阶梯度法

将目标函数$F(x)$在$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$$

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

• 若$\Delta x_k$取一阶导数,则
  $$\Delta x_k^{\ast }=-J(x_k)$$

• 若$\Delta x_k$取二阶导数,则
  $$\Delta x_k^{\ast }=\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)$$

  令上式对$\Delta x_k$导数等于0,则$\Delta x_k^{\ast }$可以取$H\Delta x_k=-J$的解.

高斯牛顿法

将$f(x_k)$而非$F(x_k)$在$x_k$附近进行泰勒展开
$$f(x_k+\Delta x_k)\approx f(x_k)+J(x_k{)}^T\Delta x_k$$

则
$$\Delta x_k^{\ast }=\arg \underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k{)}^T\Delta x_k|{|}^2$$

令上式对$\Delta x$的导数为0,得到高斯牛顿方程
$$J(x_k)f(x_k)+J(x_k)J^T(x_k)\Delta x_k=0$$

令$H(x)=J(x)J^T(x)$,$g(x)=-J(x)f(x)$,则$\Delta x_k^{\ast }$可以取$H\Delta x_k=g$的解.