基于RGBD相机的多模态结构特征融合定位 (Draft)

[TOC]

概述

目前基于RGBD相机的视觉SLAM系统，都是基于特征点或者稠密点云进行定位，计算量大，而且在无纹理或弱纹理的环境，容易跟丢，鲁棒性较差。本文提出了一种基于RGBD相机的多模态结构化数据融合定位方法，算法主要框架如下图所示，通过充分利用视觉特征，融合二维特征点、二维曲线（边缘点）、二维线段和三维平面等，结合状态估计算法进行实时、鲁邦、准确的位姿估计。在二维特征点的基础上，同时提取边缘特征，使其在弱纹理的环境也能快速、鲁棒定位；另外，在每个关键帧提取2D线段和3D平面等特征，在不增加较多计算量的情况下，引入更多稳定的视觉约束，使位姿估计更准确。

• Preprint (RG): Multimodal Structural Feature Fusion Localization Based on RGBD Camera

初始化

（1）在第一帧图像提取FAST特征点和进行边缘检测

（2）根据深度图，通过相机投影模型，得到对应的3D路标点，创建初始地图

（3）固定第一帧位姿为单位阵

数据降维

我们将输入的图像（灰度图和深度图），进行特征提取，获取一些不变的特征，为后面的状态估计提供数据支持。本算法用到的视觉特征主要包括以下四种：

• 点
  • 2D特征点
• 线
  • 2D线段
  • 2D曲线或边缘点
• 面
  • 3D平面

特征提取

2D图像特征检测

• 2D特征点：FAST / ORB
• 2D线段：LSD
• 2D曲线或边缘点：Canny边缘检测

3D点云平面提取

3D平面特征是SLAM系统中减小漂移误差的一种稳定标志。从密集点云中提取平面是一种简单的方法，常用于RGB-D相机或激光雷达。但基于稠密点云提取平面，算力较高，不利于SLAM定位的实时性；我们利用RANSAC方法从上面得到的稀疏路标点中提取3D平面。

（1）根据2D特征点和2D边缘点对应的路标点（见下面三角化章节）作为输入的3D稀疏点云

（2）随机选取三个不共线的点，计算平面参数（质心和法向量，其表示可见下面3D路标表示章节）

（3）计算稀疏点云中每个点到平面的距离，并累加，记录距离累加和、平面参数

（4）重复步骤（2）~（3），获取距离累加和最小时的平面参数

（5）收集内点：计算所有点到该平面的距离，并设定阈值，小于阈值的所有点即为平面点内点

（6）计算所有内点的均值为最终平面的 质心

（7）根据均值计算其 协方差矩阵，对该矩阵进行奇异值分解，对应奇异值最小的奇异向量即为最终平面的 法向量

example code:

• extract_domain_plane_ransac.cpp (CCV)

数据关联

我们在提取了2D特征后，需要通过2D特征间的匹配，三角化得到3D路标，并与对应的2D特征进行关联；同时，为了兼顾实时性和高性能，还要提取关键帧。

2D特征匹配

• 2D特征点：光流跟踪 / ORB描述子匹配
• 2D线段：LBD描述子匹配
• 2D曲线或边缘点（点云）：通过 距离变换 进行 倒角匹配
• 3D平面：根据两平面参数（法向量和质心），计算法向量之间的夹角以及质心间的欧式距离，作为平面匹配的相似度，据此进行匹配

3D路标及其参数化表示

3D路标点

$$P = [X, Y, Z]^T$$

（1）2D特征点和2D边缘点对应的3D路标点，初始在深度图中提取

（2）因深度图距离限制、空洞（深度缺失）以及3D路标点质量较差被剔除等因素，需要后续补充3D路标点，通过对极几何的极线搜索算法，三角化出3D路标点，如下图所示

3D直线

3D空间直线有4个自由度，其表示方式主要有两种：普吕克坐标和正交表示。

普吕克坐标

$$\mathcal{L}=\left(\mathbf{n}^{\top}, \mathbf{v}^{\top}\right)^{\top} \in \mathbb{R}^6$$

正交表示

$$(\mathrm{U}, \mathrm{W}) \in S O(3) \times S O(2) \longrightarrow \left[ \boldsymbol{\theta} \; \theta \right] \in \mathbb{R}^4$$

通常，为方便表示空间直线，我们使用普吕克坐标；但由于其具有6个变量，多于空间直线的4个自由度，在优化时会出现 过参数化问题，因此优化中使用其正交表示。

通过在两个视图图像中提取到的2D线段进行匹配，线段匹配对三角化出空间直线（普吕克坐标表示），并与2D线段进行关联。

3D平面

欧式空间3D平面有3个自由度，在本算法中对其表示方式主要有两种：Hesse形式、球坐标。

Hesse形式

$$\pi=\left(\mathbf{n}^{\top}, d\right)^{\top} \in \mathbb{R}^4$$

球坐标

$$\tau=(\theta, \phi, d)^{\top}= q(\pi)=\left(\theta=\arctan \frac{n_{y}}{n_{x}}, \; \phi=\arcsin n_{z}, \; d\right)^{\top}$$

一般优化中采用球坐标的表示方式

关键帧选取

主要根据时空两大方面，对关键帧进行选取。

（1）时间：经过一定数量的帧数

（2）空间：当当前帧距离上一关键帧的位姿变换超过一定阈值

状态估计

初始位姿估计

利用2D点特征和3D路标点，通过PNP算法，进行初始位姿估计。

后端优化

优化变量为当前帧相机位姿

$$X = \left[ t_x, t_y, t_z, \theta_x, \theta_y, \theta_z \right]^T$$

定义总体目标函数如下

$${X}^{*} = \arg \min_{X} \left\{ \sum \rho\left( \left\|\mathbf{r}_{point}\right\|^{2} \right) + \sum \rho \left( \left\|\mathbf{r}_{edge} \right\|^2 \right) + \sum \rho \left( \left\|\mathbf{r}_{line} \right\|^2 \right) + \sum \rho \left( \left\|\mathbf{r}_{plane} \right\|^2 \right) \right\}$$

特征点

（1）通过FAST或者光流法跟踪在参考关键帧图像上得到2D特征点，并得到与其关联的3D路标点。

（2）根据当前帧与参考帧间的位姿，将上述3D路标点投影到当前帧图像上，作为投影预测值。

（3）在当前帧图像上检测到的2D特征点中，找到与参考帧匹配的2D特征点，作为观测值。

（4）计算观测特征点到投影点的欧式距离，即重投影误差，作为观测残差。

残差定义为

$$r_{point} = u - K \cdot T_{cr} P_r$$

线段

（1）通过LSD算法在参考关键帧图像上提取到2D线段特征，并得到与其关联的3D空间直线。

（2）根据当前帧与参考帧间的位姿，将上述3D空间直线投影到当前帧图像上，作为投影预测值。

（3）在当前帧图像上检测到的2D线段中，找到与参考帧匹配的2D线段，作为观测值。

（4）计算观测线段两个端点到投影直线的欧式距离，即线重投影误差，作为观测残差。

残差定义为

$$r_{line} = D(z, L, T_{cw})$$

曲线或边缘点（ICP）

通过Canny边缘检测算法，在参考帧 $F{ref}$ 中检测到2D边缘点 $\mathbf{p}{i}^{\mathcal{F}_{\text {ref}}}$ ，根据相机投影模型和深度图像，得到 3D边缘点，如下

$$\mathcal{S}^{\mathcal{F}_{\text {ref }}}=\left\{\mathbf{s}_{i}^{\mathcal{F}_{\text {ref }}}\right\}=\left\{d_{i}^{\mathcal{F}_{\text {ref }}} \pi^{-1}\left(\mathbf{p}_{i}^{\mathcal{F}_{\text {ref }}}\right)\right\}$$

根据当前帧与参考帧间的位姿，将上述3D点投影到当前帧 $Fk$，得到2D点 $\mathbf{o}{i}^{\mathcal{F}_{k}}$，如下

$$\mathcal{O}^{\mathcal{F}_{k}}=\left\{\mathbf{o}_{i}^{\mathcal{F}_{k}}\right\}=\left\{\pi\left(\mathbf{R}^{\mathrm{T}}\left(\mathbf{s}_{i}^{\mathcal{F}_{\text {ref }}}-\mathbf{t}\right)\right)\right\}$$

在当前帧检测到的2D点 $P^{FK}$ 中找到 $\mathbf{o}{i}^{\mathcal{F}_{k}}$ 的最近邻点，定义公式如下

$$n\left(\mathbf{o}_{i}^{\mathcal{F}_{k}}\right)=\underset{\mathbf{p}_{j}^{\mathcal{F}_{k}} \in \mathcal{P}^{\mathcal{F}_{k}}}{\operatorname{argmin}}\left\|\mathbf{p}_{j}^{\mathcal{F}_{k}}-\mathbf{o}_{i}^{\mathcal{F}_{k}}\right\|$$

残差定义为

$$r_{edge} = \mathbf{o}_{i}^{\mathcal{F}_{k}}-n\left(\mathbf{o}_{i}^{\mathcal{F}_{k}}\right)$$

定义目标函数

$$X^*=\underset{X}{\operatorname{argmin}} \sum_{i=1}^{N}\left\|\mathbf{o}_{i}^{\mathcal{F}_{k}}-n\left(\mathbf{o}_{i}^{\mathcal{F}_{k}}\right)\right\|^{2}$$

平面

（1）通过RANSAC算法在参考关键帧提取世界坐标系的3D平面。

（2）根据参考帧的位姿 $T{rw}$ 以及当前帧与参考帧间的位姿 $T{cr}$，将上述3D平面投影到当前帧，作为预测值。

（3）在当前帧提取3D平面，找到与参考帧匹配的3D平面，作为观测值。

（4）计算观测值与预测值之间的误差，作为观测残差。

残差定义为

$$r_{plane} = q(\pi_c^w) - q(T_{cr}^{-1} T_{rw}^{-1} \pi_r^w)$$