xavier1999/vslam-data-collection-platform repository overview

该份报告将详细汇报我们在“基于多摄像头的VSLAM导航方案”项目中，截止至2023年7月12日的项目进度，该报告将详细描述我们针对视觉定位算法所采用的方案和初步结果，以及用于采集多摄像头VSLAM导航系统数据的硬件平台搭建方案和使用说明。

⁠视觉定位算法

在本章节中，我们将详细介绍本VSLAM项目中用于定位的分层定位算法（Hierarchical Localization，hloc）。其中，第1.1节简要介绍符号惯例与约定；第1.2节详细介绍hloc系统框架与工作原理；第1.3节简要汇报当前算法的性能表现。

⁠符号惯例与约定

符号惯例. 我们使用小写字母（如：k）表示标量, 粗体小写字母（如：g） 表示向量, 粗体大写字母（如：P）表示矩阵。符号g_k表示在时间戳k处向量g的值。符号P^∨为对反对称矩阵P求其所对应的向量p。
坐标系.*报告中算法涉及的估计位姿与真实位姿均是在相机坐标系中。
评价标准.我们采用绝对位姿误差（Absolute Pose Error，APE）来作为算法性能的评价指标，其反映了估计位姿和真实位姿的直接差值，可以直观地体现算法精度。APE的定义如式（1）所示，其中估计位姿P_i ∈ *SE(3)，真实位姿Q_i ∈ *SE*(3)。 F_i = Q_i⁻¹P_i 我们使用均方根误差（Root-Mean-Squared Error，RMSE）来统计APE： $$RMSE(F_i)=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\Vert log(F_i^{\vee})\Vert_2}$$

⁠大规模场景的视觉分层定位算法

在本节中，我们将介绍用于本项目的视觉定位算法。我们遵循，它描述了一种单目相机在大规模场景下由粗到细的分层定位策略。如图[hloc流程示意图]⁠所示，这种算法包含离线建库和在线定位两个阶段，首先在离线建库阶段，我们通过预先采集场景图像和对应的相机位置与姿态，获得一个包含图像检索关键帧 （Global Descriptor）、特征点匹配关键帧（Local Descriptor）的数据库。接着在车辆运行时的在线定位阶段，我们通过提取实时图像的Global Descriptor并与数据库进行匹配来实现第一步粗定位，粗定位将相机可能存在的位置范围进行了限定，因此提升了车辆在大规模场景中的定位速度。粗定位后，我们通过提取实时图像的Local Descriptor，在限定的可能位置处进行2D-3D匹配，从而得到车辆的精准位置与姿态，实现细定位。具体算法描述如下：

⁠离线建库

通过向HF-Net输入预先采集的场景图像，将Local Descriptor端的输出进行SfM（Structure-from-Motion，SfM）重建，并保存输出的Global Descriptor和SfM模型，可以实现对场景的离线建库。具体过程如下：

**建立图像检索库：**HF-Net将轻量化的MobileNet作为主干，使用逐通道进行卷积操作的深度可分离卷积来替代传统的逐像素卷积操作，精简了模型的参数量和计算量，使其更适合部署在移动设备上。在MobileNet最后一层输出的特征图将作为NetVLAD层的输入。NetVLAD是一个大型的图像检索模型，以NetVLAD为教师模型蒸馏得到的NetVLAD层将作为Global Descriptor解码器提取出输入图像的Global Descriptor。对图像及其Global Descriptor进行标号与存储，得到图像检索数据库。
**特征点提取与SfM建模：**根据所描述的，由于Local Descriptor解码器需要更高的空间分辨率来保留空间鉴别特征，而Global Descriptor又通常是从局部特征图的聚合中计算出来的，所以在HF-Net中Local Descriptor与Global Descriptor自然共享了MobileNet的一部分计算，如图[HF-Net网络结构示意图]⁠所示，MobileNet中间层的特征图输出将作为SuperPoint层的输入。SuperPoint是一个大型的图像图像特征点提取模型，以SuperPoint为教师模型蒸馏得到的SuperPoint层将作为Local Descriptor解码器提取出输入图像的Local Descriptor。特别地，本算法采用SuperGlue将采集到的图像进行两两匹配，实现SfM建模。

⁠在线定位

在车辆运行过程中，通过对车载摄像头采集到的实时图像进行特征点提取，并将其与数据库进行匹配，进行由粗到细的定位，便可以快速估计出此时摄像头的位置与姿态。具体过程如下：
**图像检索：**如图[hloc流程示意图]⁠所示，将待检索图像输入HF-Net后，可以得到该图像的Global Descriptor。由于NetVLAD是采用triplet loss来训练的，这使得在位置越接近的地方，其对应输出的Global Descriptor的L2距离越小。因此，将待检索图像的Global Descriptor与数据库中的Global Descriptor进行k近邻查找（k-Nearest Neighbors search，NN search），就可以匹配到数据库中位置与其最相近的一组图片，这组候选图片称为先验帧（Prior Frame）。由于在数据库中的图像数量远小于在SfM模型中点的数量，因此在大规模场景的定位中，采用图像检索对可能位置进行粗定位可以有效提升定位速度。
**共视聚类：**对于所有检索得到的先验帧，它们或许在图像上与待检索图片有相似之处，但不一定都在地图上的同一地点附近。因此，我们对这些先验帧在SfM模型中对应的3D点进行检查，如果两个先验帧都观察到一些共同的 3D 点，便将它们聚成一类，这些类被称为地点（Place）。
**特征点匹配与定位：**完成共视聚类后，从拥有最多先验帧的地点开始，对于每个地点，再通过计算待查图像Local Descriptor与先验帧Local Descriptor间的距离，将待查图像的特征点与当前地点中的3D点进行2D-3D匹配。然后尝试用PnP来估计相机的 6-DoF（6 degree-of-freedom，6-DoF）姿态，并进行RANSAC检验，直到第一个有效的姿态被估计出来，算法终止。

⁠实验结果

我们利用一个已有的数据集对算法进行了初步测试，该数据集是在白天城市场景下采集的一个多相机数据集，我们用朝向车体后方的相机采集到的图像及其位姿建立数据库，再用朝向车体左前方的相机采集到的图像作为检索图像对相机位姿进行重定位，APE的RMSE约为2.47509，具体结果如图[估计位姿与真实位姿对比图]⁠所示。

⁠基于多摄像头的多传感器数据采集平台

在本章节中，我们将详细介绍本项目中用于数据采集的硬件平台，本硬件平台以采集多摄像头同时图像信息为主，辅之以其他传感器来获得更丰富的真实信息，使采集到的数据集可用于各类算法的训练、测试等等。第2.1节简要介绍该平台的硬件选配方案；第2.2节详细介绍各传感器的配置方案和使用说明。

⁠数据采集平台的硬件构成

在本节中，我们将介绍构成数据采集平台各部分的硬件参数。其中包括了多摄像头系统、惯性导航系统、激光雷达和移动底盘系统。

⁠多摄像头系统

多摄像头系统包含了左右两个摄像头、4网口网卡以及多段相机滑轨。

**相机：**左右摄像头均采用Basler a2A2448-23gcBAS GigE相机，其配备1/1.8” Sony IMX547 CMOS传感器，帧速率最高可达23 fps，分辨率最高可达500像素，支持PoE供电方式或12-24V DC，支持精确时钟同步协议（Precision Time Protocol，PTP），工作温度范围在-10°C  60°C，支持C-mount镜头接口，其尺寸参数如图[相机尺寸]⁠所示。
**镜头：**左右摄像头均采用6mm工业镜头，最大光圈f2.8，最近对焦距离0.1m。
**网卡：**采用联瑞LRES2039PT-POE网卡，其提供4个支持PoE的千兆以太网口，同时兼容PCIe x8、x16通道，端口速率最高可达1000Mbps，可以很好满足多相机图像采集的需要。
**相机滑轨：**为使多摄像头系统能够采集多姿态的图像数据，本系统定制了4段滑轨以及6个传感器底座，使摄像头之间的外参可调。其中，滑轨包含垂直于系统前进方向的2段长轨道，和与系统前进方向分别呈45°、135°夹脚的2段短轨道，其尺寸参数如图[长滑轨尺寸]⁠、图[短滑轨尺寸]⁠所示。

⁠惯性导航系统

本硬件平台所搭载的惯性导航系统（Inertial Navigation System，INS）包含以下单元：惯性测量单元（Inertial Measurement Unit，IMU）、数据传输单元（Data Transfer Unit，DTU）和导航计算机。

**Xsens MTi-680G：**我们选用MTi-680G传感器作为INS的主要部分，其集成了IMU和导航计算机，其可以直接采集加速度、角速度和全球导航卫星系统（Global Navigation Satellite System，GNSS）数据，并将从IMU获取的测量数据处理成INS的位置、方向和速度进行输出。其内置的导航计算机也同时支持解算实时动态数据（Real-time Kinematic，RTK），但没有内置RTK数据采集模块。其主要参数示于表[Xsens MTi-680G主要参数]⁠。
**YeeCOM R560 4G DTU：**为获得厘米级的位置信息，我们选用YeeCOM R560作为接收RTK数据的DTU。它将来自MTi-680G的GNSS数据通过4G网络转发给Ntrip（Networked Transport of RTCM via Internet Protocol，Ntrip）服务器，再将服务器返回的RTCM（Radio Technical Commission for Maritime services，RTCM）数据转发给MTi-680G，供其进行RTK定位。其主要参数示于表[YeeCOM R560 4G DTU主要参数]⁠。

⁠激光雷达

**Livox Mid-360：**本平台搭载Livox Mid-360混合固态激光雷达进行数据采集，其视场角 （Field of View，FOV）范围如图[Livox Mid-360视场角示意图]⁠所示，为360°（水平方向）和59°（竖直方向，偏置22.5°），支持精确时钟同步协议（PTP），其主要参数示于表[Livox Mid-360主要参数]⁠。
**视场角偏置修正底座：**考虑到本项目的主要应用场景，我们需要将激光雷达安装在一个离地相对较高的位置，但Mid-360的偏置角设计是考虑将雷达放在贴近地面的位置，因此我们定制了一个倾角为23°的雷达底座，来修正其本身的偏置角，其示意图如图[视场角偏置修正底座]⁠所示。

⁠移动底盘系统

本平台的移动底盘系统由WHEELTEC 阿克曼底盘、台式主机以及车载移动电源组成，可连续进行至少1h的全传感器数据采集，能够满足大多数场景的数据采集需求，台式主机的强扩展性也使得本平台不仅可用于多传感器的数据采集，也具备了在后期对算法进行实地测试的潜力。其主要参数示于表[WHEELTEC 阿克曼底盘参数表]⁠。

⁠传感器的配置安装与使用说明

⁠数据采集软件环境配置

**拉取docker镜像：**本平台所有的传感器都可通过ROS2进行配置与数据采集，考虑到部署时的便携性，我们将各传感器的数据采集程序都打包在了一个docker镜像里，并已经将其部署在数据采集平台的车载计算机里，如果使用者需要在新的计算机上输入以下指令：

docker pull xavier1999/vslam-data-collection-platform:v1.0
docker run -it --net:host \
    --name collect_data \
    -v /data:/data \
    -v /tmp/.X11-unix:/tmp/.X11-unix \
    -v /dev:/dev \
    -e DISPLAY=$DISPLAY \
    xavier1999/vslam-data-collection-platform:v1.0

**安装pylon Camera Software Suite：**受限于系统权限，为使docker里的软件能正常运行，还需在docker外安装pylon Camera Software Suite，可登录https://www.baslerweb.com/en/downloads/software-downloads/选择对应系统的最新版本进行安装。⁠

⁠传感器的连接方式

**Basler a2A2448-23gcBAS GigE相机：**将两条网线带有螺丝的一段分别与两台相机进行连接，并将螺丝拧紧。将网线的另一端分别接入网卡正放时从左往右的第一、二个网口。
**Xsens MTi-680G与DTU：**将GNSS天线、RS232-USB converter、4pin缆线与MTi-680G相连，GNSS天线平放于水平表面，RS232-USB converter接入车载主机，4pin缆线与DTU相连（绿线接DTU RXD端，白线接DTU TXD端，黑线接DTU GND端，红线不接）。DTU电源母口接入电源适配器。
**Livox Mid-360：**将多功能线的电源端母口与电源适配器公头相连，并将网线端接入网卡正放时从左往右的第三个网口。

⁠传感器的标定

对于安装好的传感器，为了将采集到的数据进行更精细的处理，并使不同传感器间的数据可以被共享，我们还要对其进行标定，以确定它们之间的相对位置关系。我们按照如下顺序进行传感器的标定：
**拉取docker镜像：**我们使用Kalibr工具箱和Allan Variance工具箱对传感器进行标定，首先拉取Kalibr的docker镜像，并安装Allan Variance：

git clone https://github.com/ethz-asl/kalibr.git
cd kalibr
docker build -t kalibr -f Dockerfile_ros1_20_04 .
FOLDER=/path/to/your/data/on/host
xhost +local:root
docker run -it -e "DISPLAY" -e "QT_X11_NO_MITSHM=1" \
    -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    -v "$FOLDER:/data" kalibr
source /catkin_ws/devel/setup.bash
cd /catkin_ws/src
git clone https://github.com/ori-drs/allan_variance_ros.git
cd /catkin_ws & catkin build

**相机间外参与内参标定：**首先标定相机间的外参和各相机的内参，我们需要先录制好一段标定素材，然后用Kalibr进行处理。这里我们固定相机不动开始录制，匀速移动标定板并确保六个自由度都被激活、图像明亮清晰。我们还需要建立一个target.yaml文件来定义标定板的尺寸参数：

进入docker，输入以下指令进行相机内外参标定，结果将保存在camchain.yaml文件里：

**IMU内参标定：**接着标定IMU的内参，我们需要将IMU放置在一个不易打滑的水平面上，录制一段3小时以上的IMU数据。我们使用Allan Variance进行标定，首先建立配置文件config.yaml：

进入docker，输入以下指令进行IMU内参标定，结果将保存在imu.yaml文件里：

**相机与IMU间外参标定：**最后我们标定相机与IMU之间的外参，依旧使用Kalibr。我们需要重新录制一段标定素材，将IMU与相机安装固定好，保持标定板不动，移动传感器并确保六个自由度都被激活、图像明亮清晰、拿起放下传感器时没有剧烈的振动。随后进入docker，输入以下指令进行IMU内参标定，结果将分别保存在camchain-imucam.yaml和imu.yaml文件里：

⁠传感器的软件配置与数据采集

**多摄像头系统：**由于PTP配置不会被保存，因此每一次采集之前，为了获取同时刻的多相机图像信息，我们都需要先将相机与车载主机的时钟进行PTP同步，然后再开始数据采集，具体操作步骤如下：

1. 进入docker，将主机时钟设置为PTP主时钟；
2. 启动相机数据采集node程序；
3. 将相机时钟设置为PTP从时钟；
4. 将相机periodic信号设置为delay = 0，period = 50000；
5. 将相机触发器类型设置为Frame Start；
6. 启用相机触发器，将触发事件设置为图像采集，并将触发信号源选为periodic信号；
7. 对第二台相机重复操作2 6；
8. 录制bag文件，得到20fps带时间戳的图像信息。
   其样例配置指令如下：

**惯性导航系统：**惯性导航系统的传感器一经配置就会永久保存，一般情况下我们不需要重新进行配置，如果需要重新配置，参考官网的指导便可以很轻松地通过可视化软件配置。惯性导航系统的数据采集操作如下：

1. 进入docker，修改USB读写权限；
2. 启动传感器数据采集node程序；
3. 录制bag文件，得到200Hz带时间戳的加速度、角速度和位置数据。
   其样例配置指令如下：

**激光雷达：**激光雷达一经配置就会永久保存，一般情况下我们不需要重新进行配置，如果需要重新配置，参考官网的指导便可以很轻松地通过可视化软件配置。需要额外注意的是，激光雷达本身是静态ip，为使激光雷达能被程序查找到，需将对应网口的ip地址与激光雷达设置为相同的网段和子网掩码，当前的配置和激光雷达的数据采集操作如下：
激光雷达ip：169.254.80.104/16
网口ip：169.254.80.46/16

1. 进入docker，启动激光雷达数据采集node程序；
2. 录制bag文件，得到200Hz带时间戳的点云数据。
   其样例配置指令如下：