视觉SLAM14讲学习笔记-ch7

最新推荐文章于 2024-09-28 21:20:29 发布

YJD-Climb

最新推荐文章于 2024-09-28 21:20:29 发布

阅读量776

点赞数 26

文章标签：笔记机器人计算机视觉

本文链接：https://blog.csdn.net/shuaibidebei/article/details/142139659

版权

本博客主要记录学习高翔老师视觉SLAM14讲的总结笔记，若有总结不对的地方，欢迎大家与我讨论。

一、特征提取

特征点大多为图像中的角点，用于高效地表示图像中的特征，在SLAM中也称为路标。
特征点是由关键点和描述子所组成的，外观相近的特征点应该具有相似的描述子。
ORB特征提取是由FAST关键点和BRIEF描述子组成的。

1.1 FAST关键点

FAST关键点定义：如果一个像素与领域的像素差别较大，则可称为角点。一般这个领域选择为周围16个像素点，因此也成为FAST-16。
首先，进行预测试操作，位于1、5、9、13的像素值中存在3以上同时大于Ip+T或Ip-T才有可能是角点。
非极大值抑制，仅保留一定区域内极大值的角点。
尺度不变性由构建图像金字塔解决
旋转不变性由灰度质心法解决，质心表达为

$c=(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}})$

其中， $m_{pq}=\sum_{x,y\in B}x^{p}y^{q}I(x,y)$ ，特征点方向可以定义为

$\theta =\arctan (m_{01}/m_{10})$

1.2 BRIEF描述子

二进制描述子，在关键点附件随机选取128对像素点比较大小，若p>q，则为1，否则为0。结合FAST关键点中提取的方向，可得旋转不变性。

二、特征匹配

对两幅图像之间进行特征匹配，即对每个特征点之间进行描述子距离的计算，对于BRIEF描述子，通常采用汉明距离。
匹配算法通常采用暴力匹配和快速近似最近邻两种方法。

三、2D-2D对极几何

上图为对极几何示意图，通过推导可得对极约束为:

$p_{2}^{T}K^{-T}t^{\wedge }RK^{-1}p_{1}=0$

其中， $p_{1}$ 和 $p_{2}$ 表示像素坐标，K表示相机内参，t和R表示相机运动。该等式的几何意义为三角形 $O_{1}PO_{2}$ 共面。可以将上式中间的参数合并为一个矩阵，即本质矩阵：

$\begin{array}{c} x_{2}^{T}Ex_{1} = 0 \\ E=t^{\wedge }R \end{array}$

其中， $x_{1}$ 和 $x_{2}$ 表示归一化平面坐标，当然可以将内参与本质矩阵合并得到基础矩阵：

$\begin{array}{c} p_{2}^{T}Fp_{1} = 0 \\ F=K^{-T}t^{\wedge }RK^{-1} \end{array}$

3.1 本质矩阵

一般采用八点法求解E矩阵：

$\begin{bmatrix} u_{2}^{1} u_{1}^{1}& u_{2}^{1} v_{1}^{1} & u_{2}^{1} & v_{2}^{1} u_{1}^{1} & v_{2}^{1} v_{1}^{1} & v_{2}^{1} & u_{1}^{1} & v_{1}^{1} &1 \\ u_{2}^{2} u_{1}^{2}& u_{2}^{2} v_{1}^{2} & u_{2}^{2} & v_{2}^{2} u_{1}^{2} & v_{2}^{2} v_{1}^{2} & v_{2}^{2} & u_{1}^{2} & v_{1}^{2} &1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots\\ u_{2}^{8} u_{1}^{8}& u_{2}^{8} v_{1}^{8} & u_{2}^{8} & v_{2}^{8} u_{1}^{8} & v_{2}^{8} v_{1}^{8} & v_{2}^{8} & u_{1}^{8} & v_{1}^{8} &1 \end{bmatrix} \begin{bmatrix} e_{1}\\ e_{2}\\ e_{3}\\ e_{4}\\ e_{5}\\ e_{6}\\ e_{7}\\ e_{8}\\ e_{9} \end{bmatrix}=0$

通过本质矩阵求解相机运动R,t的过程是由奇异值分解SVD得到的：

$E=U\Sigma V^{T}$

$\Rightarrow \begin{matrix} t_{1}^{\wedge }=UR_{z}(\frac{\pi }{2})\Sigma U^{T} & R_{1}=UR_{z}(\frac{\pi }{2})^{T} V^{T}\\ t_{2}^{\wedge }=UR_{z}(-\frac{\pi }{2})\Sigma U^{T} &R_{2}=UR_{z}(-\frac{\pi }{2})^{T} V^{T} \end{matrix}$

分解结果存在4个解，仅有两个相机中P点都是正深度才是正确的解。而一般情况下，得到的匹配特征点是远远多于8对匹配点的，所以可以采用最小二乘法或随机采样一致性求解E。

3.2 单应矩阵

描述了两个平面之间的映射关系，自由度为8的单应矩阵可以通过4对匹配特征点算出。

3.3 尺度不确定性

单目视觉的尺度具有不确定性，所以单目SLAM常常需要初始化，初始化之后，就可以用3D-2D计算相机运动，初始化时需要一定程度上的平移，不能只有旋转。

四、三角测量

单目SLAM在得到相机间相对运动后，需要根据变换矩阵估计特征点的空间位置。可根据下式计算空间点深度：

$s_{2}x_{2}=s_{1}Rx_{1}+t\Rightarrow s_{2}x_{2}^{\wedge }x_{2}=0=s_{1}x_{2}^{\wedge }Rx_{1}+x_{2}^{\wedge }t$

其中，s1和s2为x1和x2的深度值。三角测量的矛盾在于，增大平移会导致匹配失效，减小平移而又会导致三角化精度不够。

五、3D-2D PnP

5.1 DLT

已知世界坐标系下P点坐标和在相机中投影点，则可根据下式进行求解相机运动：

$s\begin{bmatrix} u_{1} \\ v_{1} \\ 1 \end{bmatrix}= \begin{bmatrix} 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{bmatrix} \begin{bmatrix} x\\ y\\ z\\ 1 \end{bmatrix}$

转换为线性方程为：

$\left\{\begin{matrix} t_{1:4}^{T}P-t_{9:12}^{T}Pu_{1}=0\\ t_{5:8}^{T}P-t_{9:12}^{T}Pv_{1}=0 \end{matrix}\right.$

t有12个未知量，可通过6对匹配点，即可线性求解，超过6对时，可通过最小二乘求解。但由于t中未考虑 $R \in SO(3)$ 的约束，所以t举证左边3X3的矩阵块需要进行近似化，即

$R\gets (RR^{T})^{-\frac{1}{2} }R$

5.2 P3P

仅使用3对匹配点，可求得匹配点在该相机时刻下的坐标，接下来再使用ICP即可求出相机的相对运动。

5.3 最小重投影误差 BA

非线性优化方法，将相机位姿和路标坐标放在一起进行优化，像素坐标与空间点的位置关系如下：

$s_{i}\begin{bmatrix} u_{i}\\ v_{i}\\ 1 \end{bmatrix}=KT\begin{bmatrix} x_{i}\\ y_{i}\\ z_{i}\\ 1 \end{bmatrix}$

在pnp问题中，空间点位置已知，仅为优化相机位姿T的问题。构建最小二乘问题，寻找最好的相机位姿：

$T^{*}=\arg\min_{T}\frac{1}{2} \left \| u_{i} - \frac{1}{s_{i}}KTP_{i} \right \| _{2}^{2}$

上式二范数中可记为误差项 $e(x)$ ，误差项表示3D的投影位置与观测位置做差，即重投影误差。此处，将会用到李代数进行优化，用到的知识点主要有ch4和ch6。由于优化的为相机位姿，所以误差项中的x为李代数 $\xi$ ，对误差项线性化为：

$\begin{array}{c} e(\xi + \bigtriangleup \xi)\approx e(\xi)+J^{T}\bigtriangleup \xi\\ J^{T}=\frac{\partial e}{\partial \bigtriangleup \xi} \end{array}$

接下来，需要推导出雅可比矩阵J的具体表达形式。

空间点坐标 $P$ 在相机坐标系下的坐标 $P'$ 为：

$P'=(TP)_{1:3}=\begin{bmatrix} x' & y' & z' \end{bmatrix}^{T}$

$P'$ 的像素坐标为：

$\begin{array}{c} su=KP'\\ \Rightarrow \begin{bmatrix} su \\ sv \\ s \end{bmatrix}=\begin{bmatrix} f_{x} & & c_{x} \\ & f_{y} & c_{y} \\ & & 1 \end{bmatrix}\begin{bmatrix} x'\\ y'\\ z' \end{bmatrix} \\ \Rightarrow \left\{\begin{matrix} u = f_{x}\frac{x'}{z'} + c_{x} \\ v = f_{y}\frac{y'}{z'} + c_{y} \end{matrix}\right. \end{array}$

对T左乘扰动量 $\bigtriangleup \xi$ ，则e关于 $\bigtriangleup \xi$ 的求导如下：

$J^{T}=\frac{\partial e}{\partial \bigtriangleup \xi}=\frac{\partial e}{\partial P'} \frac{\partial P'}{\partial \bigtriangleup \xi}$

其中，左边部分表示为误差项关于相机坐标系下坐标求偏微分，右边部分可转换为李代数求导。左边部分可表示为如下所示：

$\frac{\partial e}{\partial P'}=-\begin{bmatrix} \frac{ f_{x}}{z'}& 0 & \frac{-f_{x}x'}{z'^{2}} \\ 0& \frac{ f_{y}}{z'} & \frac{-f_{y}y'}{z'^{2}} \end{bmatrix}$

右边部分可表示为如下所示：

$\frac{\partial P'}{\partial \bigtriangleup \xi}=\frac{\partial TP}{\partial \bigtriangleup \xi} = \begin{bmatrix} I & -P'^{\wedge } \\ 0 & 0 \end{bmatrix} _{4\times 6}= \begin{bmatrix} I & -P'^{\wedge } \end{bmatrix}$

由此，可推到出 $J^{T}$ 为（非常重要）：

$J^{T}=-\begin{bmatrix} \frac{f_{x}}{z'} & 0 & -\frac{f_{x}x'}{z'^{2}} & -\frac{f_{x}x'y'}{z'^{2}} & f_{x}+\frac{f_{x}x'^{2}}{z'^{2}} & -\frac{f_{x}y'}{z'}\\ 0 & \frac{f_{y}}{z'} & -\frac{f_{y}y'}{z'^{2}} & -f_{y}-\frac{f_{y}y'^{2}}{z'^{2}} & \frac{f_{y}x'y'}{z'^{2}} & \frac{f_{y}x'}{z'} \end{bmatrix}$

若se(3)旋转在前，平移在后，则需要将该矩阵前3列和后3列对调即可。

上面皆为对相机姿态T进行雅克比矩阵推导，同样也可用对路标点进行雅可比矩阵推导：

$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P'} \frac{\partial P'}{\partial P}$

$\frac{\partial P'}{\partial P}= \frac{\partial RP+t}{\partial P}=R$

由此可推导出误差项关于路标点的雅克比矩阵（非常重要）：

$J^{T}=-\begin{bmatrix} \frac{f_{x}}{z'} & 0 & -\frac{f_{x}x'}{z'^{2}} \\ 0 & \frac{f_{y}}{z'} & -\frac{f_{y}y'}{z'^{2}} \end{bmatrix}R$

六、3D-3D ICP

假设有一组匹配好的3D点 $P$ 和 $P'$ ，即可表示为：

$P=RP'+t$

3D-3D位姿估计问题中无需相机模型
ICP求解分为线性代数SVD和非线性优化方法BA

6.1 SVD

误差项可表示为：

$e_{i}=p_{i}-(Rp_{i}'+t)$

对很多匹配点可进行最小二乘法求解位姿：

$\begin{array}{c} \min_{R,t}\frac{1}{2} \sum_{i=1}^{n}\left \| p_{i}-(Rp_{i}'+t) \right \|_{2}^{2}\\ \Rightarrow \frac{1}{2}\sum_{i=1}^{n}(\left \| p_{i}-\bar{p}-R(p_{i}'-\bar{p'}) \right \|^{2} + \left \| \bar{p}-R\bar{p'}-t \right \| ) \end{array}$

其中， $\bar p = \frac{1}{n}\sum_{i=1}^{n}p_{i}$ ， $\bar p' = \frac{1}{n}\sum_{i=1}^{n}p_{i}^{'}$ ，所以ICP在SVD方法下分为以下三个步骤：

计算每个点的去质心坐标： $q_{i}=p_{i}-\bar p,q_{i}'=p_{i}'-\bar p'$
在最小二乘表达式中，左边部分仅与R有关，所以可以先根据下式对R进行优化求解：

$R^{*}=\arg\min_{R}\frac{1}{2} {\textstyle \sum_{i=1}^{n} \left \| q_{i}-Rq_{i}' \right \|^{2} }$

经过一系列推导可得：

$R^{*}=\arg\min tr(R {\textstyle \sum_{i=1}^{n} q_{i}'q_{i}^{T}} )$

设 $W= {\textstyle \sum_{i=1}^{n}q_{i}q_{i}'^{T}}$ ，对其进行SVD分解即可求出 $R^{*}=UV^{T}$ ，若R求得的行列式为负数，则取-R为最优值。

根据计算得到了R，即可求出t：

$t^{*}=P-R^{*}P'$

6.2 非线性优化方法

非线性优化不断迭代求解最优值，ICP对初值的要求比较宽松，其实就是扰动模型：

$\begin{array}{c} \min_{\xi}\frac{1}{2} {\textstyle \sum_{i=1}^{n}\left \| p_{i}-\exp (\xi^{\wedge})p_{i}' \right \| }\\ \Rightarrow \frac{\partial e}{\partial \bigtriangleup \xi}=-(\exp (\xi^{\wedge})p_{i}')^{\odot } \end{array}$