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

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$的解.

列文伯格-马夸尔特方法

泰勒展开只能在展开点附近才有较好的近似效果,因此应给$\Delta x$添加一个范围,称为信赖区域.

定义一个指标$\rho$刻画这个近似的好坏程度,其分子为实际函数下降的值,分母是近似模型下降的值:
$$\rho =\frac{f(x+\Delta x)-f(x)}{J(x{)}^T\Delta x}$$

通过调整$\rho$来确定信赖区域:

• 若$\rho$接近1,则近似是最好的.
• 若$\rho$太小,说明实际下降的值远小于近似下降的值,则认为近似比较差,需要缩小近似范围.
• 若$\rho$太大,说明实际下降的比预计的更大,我们可以放大近似范围.

改良版的非线性优化框架如下:

1. 给定初始值$x_0$,以及初始优化半径$\mu$

2. 对于第$k$次迭代,求解:
   $$\underset{\Delta x_k}{\min }\frac{1}{2}||f(x_k)+J(x_k{)}^T\Delta x_k|{|}^2\text{s.t.}||D\Delta x_k|{|}^2\le \mu$$

   其中,$\mu$是信赖区域的半径,$D$为系数矩阵

3. 计算$\rho$

4. 若$\rho >\frac{3}{4}$则$\mu =2\mu$

5. 若$\rho <\frac{1}{4}$则$\mu =0.5\mu$

6. 若$\rho$大于某阈值,则认为近似可行.令$x_{k+1}=x_k+\Delta x_k$

7. 判断算法是否收敛.如不收敛则返回第2步,否则结束.

第2步中$\Delta x_k$的求解要使用拉格朗日乘数法:
$$\mathcal{L}(\Delta x_k,\lambda )=\frac{1}{2}||f(x_k)+J(x_k{)}^T\Delta x_k|{|}^2+\frac{\lambda }{2}(||D\Delta x_k|{|}^2-\mu )$$

令上式对$\Delta x_k$导数为0,得到
$$(H+\lambda D^{T}D)\Delta x_k=g$$

考虑简化形式,即$D=I$,则相当于求解
$$(H+\lambda I)\Delta x_k=g$$

• 当$\lambda$较小时,$H$占主要地位,这说明二次近似模型在该范围内是比较好的,列文伯格-马夸尔特方法更接近于高斯牛顿法.
• 当$\lambda$比较大时,$\lambda I$占据主要地位,这说明二次近似模型在该范围内不够好,列文伯格-马夸尔特方法更接近于一阶梯度下降法.

ch07 视觉里程计01

特征点匹配

特征点

根据特征点匹配计算相机运动

根据特征点匹配计算相机运动.根据相机的成像原理不同,分为以下3种情况：

1. 当相机为单目时,我们只知道匹配点的像素坐标,是为2D-2D匹配,使用对极几何求解.
2. 当相机为双目或RGB-D时,我们就知道匹配点的像素坐标和深度坐标,是为3D-3D匹配,使用ICP求解.
3. 如果有3D点及其在相机的投影位置,也能估计相机的运动,是为3D-2D匹配,使用PnP求解.

2D-2D匹配: 对极几何

对极约束

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QVwt5blH-1587570602884)(1587436458419.png)]{:height=“50%” width=“50%”}

假设我们要求取两帧图像$I_1$,$I_2$之间的运动,设第一帧到第二帧的运动为$R$,$t$,两个相机中心分别为$O_1$,$O_2$.考虑$I_1$中有一个特征点$p_1$,它在$I_2$中对应着特征点$p_2$.连线$\overrightarrow{O_1{p}_1}$和$\overrightarrow{O_2{p}_2}$在三维空间中交于点$P$,这时点$O_1$,$O_2$,$P$三个点可以确定一个平面,称为极平面.$O_1{O}_2$ 连线与像平面$I_1$,$I_2$的交点分别为$e_1$,$e_2$.$e_1$,$e_2$称为极点,$O_1{O}_2$称为基线,极平面与两个像平面$I_1$,$I_2$之间的相交线$l_1$,$l_2$称为极线.

$P$在$I_1$下的相机坐标为$P=[X,Y,Z{]}^T$,两个投影像素点$p_1$,$p_2$的像素位置为$s_1{p}_1=KP$,$s_2{p}_2=K(RP+t)$.

取$p_1$,$p_2$的归一化坐标$x_1=K^{-1}{p}_1$,$x_1=K^{-1}{p}_2$,则可以推得$x_2\simeq R{x}_1+t$.上式中$\simeq$表示尺度意义上相等,即在齐次坐标下是相等的,物理上表示对原点成投影关系.

经过推导,得到:
$$x_2^T{t}^{\wedge }R{x}_1=0\text{\hspace{1em}(1)}$$
代入$p_1$,$p_2$,得到:
$$p_2^T{K}^{-T}{t}^{\wedge }R{K}^{-1}{p}_1\text{\hspace{1em}(2)}$$
式$(1)$和式$(2)$都称为对极约束,定义基础矩阵$F$和本质矩阵$E$​,可以进一步简化对极约束:
$$E=t^{\wedge }RF=K^{-T}E{K}^{-1}{x}_2^{T}E{x}_1=p_2^{T}F{p}_1=0\text{\hspace{1em}(3)}$$
由于$E$与$F$之间只差了相机内参,相机内参是已知的,因此实践中往往使用形式更简单的$E$.

本质矩阵$E$的求解

考虑到$E$的尺度等价性,可以用8对点来估计$E$,是为八点法.

对于一对匹配点,其归一化坐标$x_1=[u_1,v_1,1{]}^T$,$x_2=[u_2,v_2,1{]}^T$.根据对极约束,有
$$(u_1,v_1,1)\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=0$$
把矩阵$E$展开为向量$e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9{]}^T$,对极约束可以写成与$e$有关的线性形式:
$$[u_1{u}_2,u_1{v}_2,u_1,v_1{u}_2,v_1{v}_2,v_2,u_2,v_2,1]\cdot e=0$$
把八对点对应的$x_1$,$x_2$分别代入方程中,得到线性方程组:
$$\left(\begin{array}{ccccccccc}{u}_1^1{u}_2^1& u_1^1{v}_2^1& u_1^1& v_1^1{u}_2^1& v_1^1{v}_2^1& v_2^1& u_2^1& v_2^1& 1\\ u_1^1{u}_2^2& u_1^2{v}_2^2& u_1^2& v_1^2{u}_2^2& v_1^2{v}_2^2& v_2^2& u_2^2& v_2^2& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ u_1^1{u}_2^8& u_1^8{v}_2^8& u_1^8& v_1^8{u}_2^8& v_1^8{v}_2^8& v_2^8& u_2^8& v_2^8& 1\end{array}\right)\left(\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0$$
求得E后,对$E$进行SVD分解以求取$R$,$t$:设$E$的SVD分解为$E=U\Sigma V^T$,则对应的$R$,$t$分别为:

$$t^{\wedge }=U{R}_Z(\frac{\pi }{2})\Sigma U^{T}R=U{R}_Z^T(\frac{\pi }{2})\Sigma U^T$$
其中$R_Z(\frac{\pi }{2})$表示沿$Z$轴旋转90°得到的旋转矩阵.

对极几何的讨论

1. 尺度不确定性: 2D图像不具有深度信息,这导致了单目视觉的尺度不确定性.

   实践中设$t$为单位1,计算相机运动和和特征点的3D位置,这被称为单目SLAM的初始化.

2. 初始化的纯旋转问题: 若相机发生纯旋转,导致$t$为零,得到的$E$也将为零,会导致我们无从求解R.因此单目初始化不能只有纯旋转,必须要有一定程度的平移.

3. 多于8对点的情况:

   对于八点法,有$Ae=0$,其中$A$为一个8×9的矩阵.

   若匹配点的个数多于8个,$A$的尺寸变化,上述方程不成立.因此转而求取最小化二次型
   $$\underset{e}{\min }||Ae|{|}_2^2=\underset{e}{\min }{e}^T{A}^{T}Ae$$

   是为最小二乘意义下的$E$矩阵.

3D-2D匹配: PnP(Perspective-n-Point)

2D-2D的对极几何方法需要8个或8个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题。然而，如果两张图像中其中一张特征点的3D位置已知，那么最少只需3个点对（需要至少一个额外点验证结果）就可以估计相机运动。

在双目或RGB-D的视觉里程计中，我们可以直接使用PnP估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。

PnP问题有多种解决方法:

1. 直接线性表变换(DLT): 先求解相机位姿,再求解空间点位置
2. P3P: 先求解空间点位置,再求解相机位姿
3. Bundle Adjustment: 最小化重投影误差,同时求解空间点位置和相机位姿

直接线性变换(DLT): 先求解相机位姿,再求解空间点位置

考虑某个空间点$P$的齐次世界坐标为$P=(X,Y,Z,1{)}^T$ .在图像$I_1$中投影到特征点的归一化像素坐标$x_1=(u_1,v_1,1{)}^T$.此时相机的位姿$R$,$t$是未知的,定义增广矩阵$[R|t]$(不同于变换矩阵$T$)为一个3×4的矩阵,包含了旋转与平移信息,展开形式如下:
$$s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$

用最后一行把s消去,得到两个约束:
$$\{\begin{array}{r}{\mathbf{t}}_1^{T}P-{\mathbf{t}}_3^{T}P{u}_1=0\\ {\mathbf{t}}_2^{T}P-{\mathbf{t}}_3^{T}P{v}_1=0\end{array}$$

其中${\mathbf{t}}_1=(t_1,t_2,t_3,t_4{)}^T$,${\mathbf{t}}_2=(t_5,t_6,t_7,t_8{)}^T$,${\mathbf{t}}_3=(t_9,t_{10},t_{11},t_{12}{)}^T$.${\mathbf{t}}_1$,${\mathbf{t}}_2$,${\mathbf{t}}_3$为待求量.

将$N$对匹配的特征点代入方程中,得到线性方程组:
$$\left(\begin{array}{ccc}{P}_1^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right)\left(\begin{array}{c}{\mathbf{t}}_1\\ {\mathbf{t}}_2\\ {\mathbf{t}}_3\end{array}\right)=0$$

只需6对匹配点即可求解增广矩阵$[R|t]$,若匹配点数多于6对时,可以求最小二乘解.对于求解出的旋转矩阵$R$,可以通过QR分解等手段将其投影到$SE(3)$上.

P3P: 先求解空间点位置,再求解相机位姿

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-9IuduXXH-1587570602886)(1587451327097.png)]

已知3对匹配点的世界坐标$A$,$B$,$C$和投影坐标$a$,$b$,$c$,根据三角形的余弦定理,有
$$\{\begin{array}{r}O{A}^2+O{B}^2-2OA\cdot OB\cdot \cos ⟨a,b⟩=A{B}^2\\ O{B}^2+O{C}^2-2OB\cdot OC\cdot \cos ⟨b,c⟩=B{C}^2\\ O{A}^2+O{C}^2-2OA\cdot OC\cdot \cos ⟨a,c⟩=A{C}^2\end{array}$$

记$x=OA/OC$,$y=OB/OC$,$u=B{C}^2/A{B}^2$,$v=A{C}^2/A{B}^2$
$$\{\begin{array}{rl}(1-u)y^2-u{x}^2-\cos ⟨b,c⟩y+2uxy\cos ⟨a,b⟩+1& =0\\ (1-w)x^2-w{y}^2-\cos ⟨a,c⟩y+2wxy\cos ⟨a,b⟩+1& =0\end{array}$$

上式中,三个余弦角$\cos ⟨a,b⟩$,$\cos ⟨b,c⟩$,$\cos ⟨a,c⟩$以及$u$,$v$是已知的,可以求解出$x$,$y$,进而求解出$A$,$B$,$C$三点的相机坐标.然后根据3D-3D的点对,计算相机的运动$R$,$t$.

Bundle Adjustment: 最小化重投影误差,同时求解空间点位置和相机位姿

设相机位姿变换矩阵$T$,某空间点的世界坐标$P_i=[X_i,Y_i,Z_i{]}^T$,其投影的像素坐标为${\mathbf{u}}_i=[u_i,v_i{]}^T$,像素位置与空间点位置的关系如下:
$$s_i{\mathbf{u}}_i=KT{P}_i$$

由于相机位姿未知及观测点的噪声,上式存在一个误差,称为重投影误差$e=u_i-\frac{1}{s_i}KT{P}_i$.因此我们对重投影误差求和,寻找最好的相机位姿和特征点的空间位置,最小化重投影误差:
$$\begin{gathered} T^{\ast }=\arg \underset{T}{\min }\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}KT{P}_i|{|}^2 \\ {P}_i^{\ast }=\arg \underset{P_i}{\min }\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}KT{P}_i|{|}^2 \end{gathered}$$

使用最小二乘优化,要分别求$e$对$T$和$P$的导数:
$$e(x+\Delta x)\approx e(x)+J\Delta x$$

• 求$e$对$T$的导数:

  当$e$为像素坐标误差(2维),$x$为相机位姿(6维)时,$J$将是一个2×6的矩阵.我们来推导$J$的形式:

  取中间变量$P^{\prime }=(TP{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$

  使用李代数求导的扰动模型,对$T$左乘微小扰动$\delta \xi$,求导得到:
  $$\frac{\partial e}{\partial \delta \xi }=\underset{\delta \xi =0}{\lim }\frac{e(\delta \xi \oplus \xi )-e(\xi )}{\delta \xi }=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta \xi }$$

  其中的$\oplus$表示李代数的左乘扰动

  其中第一项$\frac{\partial e}{\partial P^{\prime }}$:
  $$\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime }}& \frac{\partial u}{\partial Y^{\prime }}& \frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}& \frac{\partial v}{\partial Y^{\prime }}& \frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]$$

  第二项$\frac{\partial P^{\prime }}{\partial \delta \xi }$为变换后的点关于李代数的导数:
  $$\frac{\partial P^{\prime }}{\partial \delta \xi }=\frac{(TP)}{\partial \delta \xi }=(TP{)}^{\odot }=\left[\begin{array}{cc}I& -P^{\prime \wedge }\\ 0^T& 0^T\end{array}\right]$$

  在$P^{\prime }$定义中,取出前三维,得到
  $$\frac{\partial P^{\prime }}{\partial \delta \xi }=[I,-P^{\prime \wedge }]$$

  将两项相乘,得到了2×6的雅可比矩阵$J^T$
  $$J^T=\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$

• 求$e$对$P$的导数

3D-3D匹配: ICP

对于一组已配对好的3D点:
P = { p 1 , ⋯   , p n } , P ′ = { p 1 ′ , ⋯   , p n ′ } P = \{p_1, \cdots ,p_n\}, \quad P' = \{p_1', \cdots, p_n'\} P={p1​,⋯,pn​},P′={p1′​,⋯,pn′​}

现在,想要找一个欧氏变换$R$,$t$,使得:
$$\forall i,p_i=R{p}_i^{\prime }+t$$

ICP问题的求解包含两种方式:

1. 利用线性代数的求解(主要是SVD)
2. 利用非线性优化方式的求解(类似于Bundle Adjustment)

SVD方法

定义第$i$对点的误差项为$e_i=p_i-(R{p}_i^{\prime }+t)$,定义两组点的质心$p=\frac{1}{n}{\sum }_{i=1}^n(p_i)$,$p^{\prime }=\frac{1}{n}{\sum }_{i=1}^n(p_i^{\prime })$

构建最小二乘问题,求取最合适的$R$,$t$.
$$\begin{array}{rl}\underset{R,t}{\min }J& =\frac{1}{2}\sum _{i=1}^n||(p_i-(R{p}_i^{\prime }+t))|{|}_2^2\\ & =\frac{1}{2}\sum _{i=1}^n||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2+||p-R{p}^{\prime }-t|{|}^2\end{array}$$

左边只和旋转矩阵$R$相关,而右边既有$R$也有$t$,但只和质心相关.因此令左边取最小值解出$R$,代入到右边令式子等于0求出$t$.

定义去质心坐标$q_i=p_i-p$,$q_i^{\prime }=p_i^{\prime }-p^{\prime }$,则优化目标可写成:
$$\begin{array}{rl}{R}^{\ast }& =\underset{R}{\min }\sum _{i=1}^n||p_i-p-R(p_i^{\prime }-p^{\prime })|{|}^2\\ & =\underset{R}{\min }\sum _{i=1}^n-q_i^{T}R{q}_i^{\prime }\\ & =-tr\left(R\sum _{i=1}^n{q}_i^{\prime }{q}_i^T\right)\end{array}$$
省略数学证明,定义矩阵:
$$W=\sum _{i=1}^n{q}_i{q}_i^{\prime T}$$
对矩阵$W$进行SVD分解得到:
$$W=U\Sigma V^T$$
可求解
$$R=U{V}^T$$

非线性优化方法

使用李代数表达表达位姿,目标函数可以写成
$$\underset{\xi }{\min }=\frac{1}{2}\sum _{i=1}^n||(p_i-\exp ({\xi }^{\wedge })p_i^{\prime })|{|}_2^2$$
误差项关于位姿的导数可以用李代数求导的扰动模型,计算导数得到:
$$\frac{\partial e}{\partial \delta \xi }=-(\exp ({\xi }^{\wedge })p_i^{\prime }{)}^{\odot }$$
可以直接使用最小二乘优化方法求解位姿.